Velocity Control

[jz...@swri.org]

MoveIt Gurus,

 Is it possible to specify the desired velocity for generated trajectories?

 For many applications, it is important to limit the robot’s velocity for certain motion segments.  These limits would not necessarily be applied “permanently” for all moves, but only for specific operations.  The constraints may either be for the cartesian velocity or the joint-level velocity, depending on the application.  For example, a painting robot might need to sweep across a surface at constant linear velocity.  Or, in a human-collaboration task, the robot might be limited to 10% of max joint speeds.

 For previous arm-nav applications, we have adjusted the velocity by artificially reducing the velocity limits in joint_limits.yaml.  But this is undesirable for several reasons:

  1) it does not support cartesian velocity control
  2) it is mostly a “global” limit, and is awkward to adjust for different motion segments
  3) Ideally, the velocity control should operate “inside” the URDF velocity limits for a given robot

 Any suggestions on how to achieve velocity control within the MoveIt framework?

 Thanks,
  Jeremy Zoss
  Southwest Research Institute 

[Ioan Sucan]

Hello Jeremy,

That is an interesting problem. I presume you would do this on a per application basis (and not want to change the settings between consecutive requests). In that case, I would use a planning request adapter in the planning pipeline. That would essentially take the trajectory (which runs at maximum allowed speed)  and decrease the velocity throughout.

Does this sound reasonable?

If you want to do this on a per request basis, we can include a velocity scaling factor in the planning request.

This is all for velocities specified per joint. If you want to specify velocities in Cartesian space, then we need an additional way to parametrize the trajectory (which we do not currently have). This could be added to the trajectory_processing library in moveit_core; then we could apply it to trajectories using the planning request adapters as well.

Ioan

[Sachin Chitta]

Hey Jeremy,

MoveIt! does processing on the path to output trajectories that respect joint velocity limits. We should be able to handle cartesian limits as well although as Ioan mentioned there's no easy way to specify them easily currently. We can modify our current trajectory processing to do this (and we may actually do this over the next few weeks since we need it in a different application too).

Sachin

--
Sachin Chitta
Manager and Research Scientist
Autonomous Mobile Manipulation Group
Willow Garage

[jz...@swri.org]

Ioan & Sachin,

I think this capability would need to be available on a per-request basis, rather than a per-application basis.  As an example, you may want the robot to move "slow" when it has a part gripped (e.g. to avoid dropping it) but move "fast" when the gripper is empty and you are repositioning for a new operation.

I think cartesian velocity control is definitely a desirable feature.  Let me know if you'd like me to post an enhancement request to the github "Issues" list, for tracking purposes.

In reponse to Sachin's comment:
I'm aware that the current trajectories are limited to the specified robot joint-velocity-limits.  I think there is still a need to limit joint velocities to values lower than the robot limits, for individual motion segments.  I will investigate Ioan's Planning Request Adapter suggestion, but I would still consider adding per-MotionPlanRequest Joint Velocity control to the "enhancement request" list.

Thanks for your feedback!
  - Jeremy

[Thiago Motta]

Is there any news on this ?

[Sachin Chitta]

In MoveIt!, the limits for individual joints are referred to as "bounds" and can be set at the joint model level (there is a setVariableBounds function inside joint model). This will allow you to change the bounds if you need to (you will have to get access to a non-const RobotModel to be able to do this. In normal operation, the bounds are automatically loaded using a robot_model_loader from the joint_limits.yaml file information (and the URDF). The trajectory processing module in MoveIt! (also inside moveit_core) lets you apply these bounds on a joint trajectory giving you velocity and acceleration bounded trajectories. Note that this is at a joint level and not at a cartesian level.

For cartesian velocity bounds, there is no direct implementation in MoveIt! yet but there are other places you can get this from (e.g. I believe KDL does this).

Sachin

[Patrick Goebel]

Hi Thiago,

There is a related open enhancement request posted on Github.  In one of my comments, I include the following Python code to adjust the speed of a trajectory.  Note that this technique scales in the speed in joint space.  The way you would use it would be something like the following:

traj = right_arm.plan()
scaled_traj = scale_trajectory_speed(traj, 0.5)
right_arm.execute(scaled_traj)

Here is the full listing:

#!/usr/bin/env python

import rospy
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from geometry_msgs.msg import PoseStamped, Pose

def scale_trajectory_speed(traj, scale):
       # Create a new trajectory object
       new_traj = RobotTrajectory()
      
       # Initialize the new trajectory to be the same as the planned trajectory
       new_traj.joint_trajectory = traj.joint_trajectory
      
       # Get the number of joints involved
       n_joints = len(traj.joint_trajectory.joint_names)
      
       # Get the number of points on the trajectory
       n_points = len(traj.joint_trajectory.points)
       
       # Store the trajectory points
       points = list(traj.joint_trajectory.points)
      
       # Cycle through all points and scale the time from start, speed and acceleration
       for i in range(n_points):
           point = JointTrajectoryPoint()
           point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
           point.velocities = list(traj.joint_trajectory.points[i].velocities)
           point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
           point.positions = traj.joint_trajectory.points[i].positions
                        
           for j in range(n_joints):
               point.velocities[j] = point.velocities[j] * scale
               point.accelerations[j] = point.accelerations[j] * scale * scale
           
           points[i] = point

       # Assign the modified points to the new trajectory
       new_traj.joint_trajectory.points = points

       # Return the new trajecotry
       return new_traj
  
      

--patrick