Fix wrong behaviors in :go-pos-unsafe / :move-trajectory / :move-trajectory

[furushchev]

• :go-pos-unsafe internally computes the duration of base movement given a goal and slow the motion if it is less than 1000 msec (for safety reason, I guess), but the duration is still left original value so the goal is never reached.
• :move-trajectory: documentation says

  (:move-trajectory
   (x y d &optional (msec 1000) &key (stop t) (start-time) (send-action nil))
" x [m/sec] y [m/sec] d [rad/sec] msec [milli second]

But it also accepts list for x and y but in that case, x is assumed as the list of #f(x y d) , y for list of time[msec], and then d and msec are ignored!
This is really confusing...
It looks it is the function that :move-trajectory-sequence should have, so I moved the functionality to :move-trajectory-sequence.
And I also fixed some parts where msec and sec are mistaken in original functionality moved from :move-trajectory.

• Added units in documentation in :move-trajectory-sequence

-   "trajectory-points [ list of #f(x y d) ] time-list [list of time span]
+   "trajectory-points [ list of #f(x y d) ([m/sec] for x, y; [rad/sec] for d) ]
+    time-list [list of time span [msec] ]

[furushchev]

Expected changes in practical use:

• when :go-pos-unsafe is used, base moves slower than before (Though it was incorrect speed before)

[furushchev]

Hmm, I'm confused... It looks like the original :move-trajectory-sequence takes trajectory-points as list of x [m], y [m], d [rad] on each time points (given as time-list), instead of x[m/s], y[m/s], d [rad/s]...

@k-okada What are :move-trajectory / :move-trajectory-sequence for? Could you give me any pointer if you know?

[furushchev]

@k-okada Sorry, searching github issue, the original author is @YoheiKakiuchi

[furushchev]

My questions are now:

1. What is the difference between :go-velocity and :move-trajectory?

  (:go-velocity
   (x y d ;; [m/sec] [m/sec] [rad/sec]                                                                
    &optional (msec 1000) ;; msec is total animation time [msec]                                      
    &key (stop t) (wait))
   "move robot at given speed for given msec.                                                         
    if stop is t, robot stops after msec, use wait t for blocking call                                
    return nil if aborted while waiting by enabling :wait, otherwise return t"

  (:move-trajectory
   (x y d &optional (msec 1000) &key (stop t) (start-time) (send-action nil))
" x [m/sec] y [m/sec] d [rad/sec] msec [milli second]

2. :move-trajectory-sequence is just a sequence version of :move-trajectory?

If true, it should be ok to define :move-trajectory as follows?:

  (:move-trajectory
   (x y d &optional (msec 1000) &key (stop t) (start-time) (send-action nil))
" x [m/sec] y [m/sec] d [rad/sec] msec [milli second]                                                 
stop [ stop after msec moveing ]                                                                      
start-time [ robot will move at start-time ]                                                          
send-action [ send message to action server, it means robot will move ]"
   (send self :move-trajectory-sequence
         (list (float-vector x y d))
         (list msec)
         :stop stop :start-time start-time :send-action send-action))

3. Is there any place that these methods are used?

[furushchev]

OK. I now fixed implementation following documentation as follows:

  (:go-velocity
   (x y d ;; [m/sec] [m/sec] [rad/sec]                                                                
    &optional (msec 1000) ;; msec is total animation time [msec]                                      
    &key (stop t) (wait))
   "move robot at given speed for given msec.                                                         
    if stop is t, robot stops after msec, use wait t for blocking call                                
    return nil if aborted while waiting by enabling :wait, otherwise return t"

  (:move-trajectory
   (x y d &optional (msec 1000) &key (stop t) (start-time) (send-action nil))
- " x [m/sec] y [m/sec] d [rad/sec] msec [milli second]
+ " x [m] y [m] d [rad] msec [milli second]
    stop [ stop after msec moveing ]
    start-time [ robot will move at start-time [sec or ros::Time] ]
    send-action [ send message to action server, it means robot will move ]"

  (:move-trajectory-sequence
   (trajectory-points time-list &key (stop t) (start-time) (send-action nil))
   "Move base following the trajectory points at each time points
    trajectory-points [ list of #f(x y d) ([m] for x, y; [rad] for d) ]
    time-list [list of time span [msec] ]
    stop [ stop after msec moveing ]
    start-time [ robot will move at start-time [sec or ros::Time] ]
    send-action [ send message to action server, it means robot will move ]"

[k-okada]

You're right.

A go-velocity is based on historical reason, go-pos and go-velocity interface of hrpsys (https://github.com/fkanehiro/hrpsys-base/blob/315.15.0/idl/AutoBalancerService.idl#L336-L355). And :move-trajectory and :move-trajectory-sequence is based on ROS interface, like send-trajectory (https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/0.3.13/pr2eus/robot-interface.l#L931) , so which is basically private method and not inted to directory called from users.

Since we do not have go-pos-sequence interface in hrpsys (they use setFootSteps : https://github.com/fkanehiro/hrpsys-base/blob/315.15.0/idl/AutoBalancerService.idl#L370 ), we'll export move-trajectory-sequence as public method, but I am not sure if we need move-trajectory ? If we still need them, may be the interface should be (trajectory-point time &key (stop t) (start-time) (send-action nil)). Does anyone uses this interface?

c.f.
6bd4fb3
#52

[k-okada]

BTW is it possible to make test code for this?

[furushchev]

@k-okada Thanks for clear explanation!
About testing, we now have almost isolated code base both on simulation and on real robot, so we ideally need to test on both cases.
Testing on simulation is relatively easy than on real robot because testing on real robot requires at least pr2_base_trajectory_action which is internally used (possible + pr2_gazebo)

[furushchev]

@k-okada ping or any comments?

[k-okada]

I think @708yamaguchi is checking this feature with real robot, and waiting for feedback from him

[takayuki5168]

@k-okada cc @furushchev
I checked with the real robot, and everything worked well !