Controlling trajectory speed

[Patrick Goebel]

For one of my applications, I would like to move the arm as quickly as
possible along the planned trajectory. Looking at both the Python and
C++ API, I don't see a simple function for setting the goal time for the
computed trajectory. Is there a recommended way to do this?

Thanks,
patrick

[Patrick Goebel]

So I think I have a workable answer to my own question. Using the
Python API, the following code snippet appears to alter the trajectory
speed as desired (here the speed is doubled):

traj = right_arm.plan()
new_traj = RobotTrajectory()
new_traj.joint_trajectory = traj.joint_trajectory
n_joints = len(traj.joint_trajectory.joint_names)
n_points = len(traj.joint_trajectory.points)

spd = 2.0

for i in range(n_points):
traj.joint_trajectory.points[i].time_from_start =
traj.joint_trajectory.points[i].time_from_start / spd
for j in range(n_joints):
new_traj.joint_trajectory.points[i].velocities[j] =
traj.joint_trajectory.points[i].velocities[j] * spd
new_traj.joint_trajectory.points[i].accelerations[j] =
traj.joint_trajectory.points[i].accelerations[j] * spd
new_traj.joint_trajectory.points[i].positions[j] =
traj.joint_trajectory.points[i].positions[j]

self.right_arm.execute(new_traj)

Not sure if it makes sense to add a parameter to the plan() or execute()
functions for trajectory speed that would do essentially what I have done?

--patrick

[Ioan Sucan]

Hello Peter,

I was postponing my answer to your question in the hope to add the functionality you implemented on your own :)

Code looks good. I will make this a parameter though; I am thinking something that can be set using dynamic reconfigure.

Ioan

[Patrick Goebel]

Cool--and thanks!

--patrick

[Ioan Sucan]

I added a ROS parameter for this (dynamic reconfigure, under trajectory_execution).

Let me know if you'd rather make this an argument in the planning request message.

Ioan

[jz...@swri.org]

I agree that some "better" method should be provided for trajectory velocity control.  See my earlier post on "Velocity Control" from 5/22.  But I worry that the approach outlined below might be too simplistic.  Further discussion may be required.

In my earlier post, Ioan suggested that I use a PlanningRequestAdapter to implement joint-level velocity controls.  This method would allow the reduced-velocity trajectories to be run through the existing trajectory filters, ensuring that the resulting trajectory meets the requirements for smooth, consistent trajectories that fall within the robot's velocity/accel limits.  I'm not sure the math provided below, scaling the time/velocity/accel by a constant factor, generates correct internally-consistent trajectories.  Something more along the lines of the existing trajectory filters may be required.

Some other areas worth discussing:

  1) Interface

Where will the user specify the desired velocity?  A ROS param allows easy setting of "global" overrides, but it might be required to specify a different velocity for each trajectory plan.  This gets messy with parameters.  It could be incorporated into the MotionPlanRequest message, which would capture this nicely.  This could also be exposed through the move_group_interface plan() and execute() calls, if desired.  Do we need some way to specify both global (default) and request-specific (optional) velocities?

  2) Granularity

What level of specificity is required?  Do we need control over the planned velocities of each joint?  Is a single value sufficient for the entire system?  Do we want to set a different value for each planning group?  My vote is that it may be helpful to use different velocity scaling factors for each group, but not each joint.  The MotionPlanRequest method captures this as well.

  3) Absolute vs. Relative

Most industrial robots specify joint velocity using a single value for each move command.  This value is typically 0-100%, which is interpreted to mean that no joint will exceed that percentage of its maximum velocity limit.  The trajectory is planned so that all joints reach the goal at the same time.  We could, alternatively, specify an absolute joint-velocity limit (XX rad/sec), but I think the former is easier to use.

  4) Range

Peter mentioned using a scaling factor of 2.0.  I thought the normal operation of MoveIt was to plan a trajectory that matches the maximum joint-velocities of the robot.  Does it make sense, then, to allow scaling factors outside the range of 0-100%?  If factors of >100% are required, then maybe there's some other error in the configuration or trajectory-generation pipeline...

Sorry for the long message.  I see Ioan's already implementing some form of velocity control.  I just think that this is an important capability, and I would rather see it implemented correctly than quickly.  Otherwise, we get stuck with having to break or maintain "legacy" interfaces.

Thanks for reading,
  Jeremy Zoss
  Southwest Research Institute

[Patrick Goebel]

Hi Ioan,

That's a good question.  I can envision needing to change up the speed of the trajectory fairly frequently as the robot moves in and out of tight places.  So perhaps it would make more sense to make it an argument?

--patrick

[Ioan Sucan]

For now, the thing that is done is really just applying a multiplicative factor to velocities. This probably will produce a path that does not meet acceleration constraints, as the factor is applied _after_ the time parametrization.

Right now we have velocity limits loaded from the URDF, and they can be overridden using joint_limits.yaml; Now we are talking about adding a third way of specifying velocity limits. This is not really a problem, but we need to make things consistent and in a way that makes sense to new users.

I think Jeremy makes a number of very good points to discuss on.

The first (interface) is already an issue. I think we should have a "global" set of limits, which is obtained from URDF + joint_limits.yaml; These should not be changeable at runtime.

The "local" set of limits should probably be part of a planning request. Thoughts?

In terms of granularity, I think we should not do things in terms of scaling, but specify explicitly the joint limits desired (we can skip the joints that have global joint limits that we are fine with)

This would then not be done with a scaling factor (although the user can simply pass in the scaled global limits)

How does this sound?

In terms of unit of measurement, I think things like range can be handled in the moveit_controller_manager implementation. For the joint limits themselves I think we should always use SI.

Ioan

[Adolfo Rodríguez Tsouroukdissian]

On Thu, Jun 20, 2013 at 6:10 PM, Ioan Sucan <isu...@willowgarage.com> wrote:

For now, the thing that is done is really just applying a multiplicative factor to velocities. This probably will produce a path that does not meet acceleration constraints, as the factor is applied _after_ the time parametrization.

Right now we have velocity limits loaded from the URDF, and they can be overridden using joint_limits.yaml; Now we are talking about adding a third way of specifying velocity limits. This is not really a problem, but we need to make things consistent and in a way that makes sense to new users.

A velocity scaling factor would basically mean retiming the resulting motion plan, so it takes longer to execute, right?. This could even be done after the time parametrization, if it's more convenient implementation-wise.

A scaling factor is very easy to specify, just a scalar. A complete set of joint limits specified in the motion plan request (optionally) is less trivial to setup. I wonder how common this usecase is.

Adolfo.

--
Adolfo Rodríguez Tsouroukdissian
Senior robotics engineer
adolfo.r...@pal-robotics.com
http://www.pal-robotics.com

PAL ROBOTICS S.L
c/ Pujades 77-79, 4º4ª
08005 Barcelona, Spain.
Tel. +34.93.414.53.47
Fax.+34.93.209.11.09
Skype: adolfo.pal-robotics
Facebook - Twitter - PAL Robotics YouTube Channel

AVISO DE CONFIDENCIALIDAD: Este mensaje y sus documentos adjuntos, pueden contener información privilegiada y/o confidencial que está dirigida exclusivamente a su destinatario. Si usted recibe este mensaje y no es el destinatario indicado, o el empleado encargado de su entrega a dicha persona, por favor, notifíquelo inmediatamente y remita el mensaje original a la dirección de correo electrónico indicada. Cualquier copia, uso o distribución no autorizados de esta comunicación queda estrictamente prohibida.

CONFIDENTIALITY NOTICE: This e-mail and the accompanying document(s) may contain confidential information which is privileged and intended only for the individual or entity to whom they are addressed.  If you are not the intended recipient, you are hereby notified that any disclosure, copying, distribution or use of this e-mail and/or accompanying document(s) is strictly prohibited.  If you have received this e-mail in error, please immediately notify the sender at the above e-mail address.

[jz...@swri.org]

Velocity limits are provided in multiple locations, but I think there are valid reasons:
  - joint_limits.yaml is needed because URDF only provides velocity limits (not accel limits)
  - both velocity and accel limits are needed for trajectory filtering
  - both URDF and joint_limits.yaml specify maximum robot capabilities.  These should not be changeable at runtime.
  - separate interface is needed to specify desired trajectory velocity for each motion

It would be the most explicit/flexible to do as Ioan suggests and specify absolute maximum joint velocities for each joint in the MotionPlanRequest.  However, I think that's overkill, and that most applications will not need that level of detail.  In my experience, you don't specify desired joint velocities to get precise control over your motion.  At the joint-level, that's fairly abstracted from the actual manipulation task.  Instead, you typically specify joint velocities to "scale" the overall motion speed: Fast, Slow, intermediate.  You might initially run an application with all joints de-rated to 10% or 25% of maximum speed, until you're confident in the robot's control logic.  Then, you can gradually speed up the motions to 100% speed.  In my opinion, a per-group scaling factor (not per-joint absolute limit) is sufficient.  This could be implemented as a wrapper around a lower-level per-joint absolute velocity limit, but I think that's needlessly complex.

My preference:
  - add a joint_velocity_scale field to the MotionPlanRequest (or maybe TrajectoryConstraints?)
      - valid range is 0-1  (0 means 100% or "use default", or similar)
  - add a new PlanningRequestAdapter (or enhance existing TimeParameterization) to limit trajectory velocities to joint_velocity_scale * joint_max_vel[i]
  - add new method to move_group_interface: set_joint_velocity_scale()
  - [optional] add a ROS param default_joint_velocity_scale.  If MotionPlanRequest.joint_velocity_scale == 0, use the default_joint_velocity_scale instead.

Ideally, I'd like to be able to specify cartesian velocity limits for the end-effector as well, but I'm okay if we want to leave that discussion for later.

- Jeremy

[Patrick Goebel]

Everything Jeremy has said makes sense to me.  In my case, I'm guessing my most common need will be overall scaling from 0-100% of maximum.

--patrick

[Patrick Goebel]

I should have thought of this earlier: would it also be possible to add a planning argument that specifies the *time* to execute the trajectory?  This could be given either as a ROS duration or a specific ROS time.  For example, suppose I want the gripper to intercept a moving object that I am tracking by some means and can therefore predict where it will be in the near future.  The I would know (roughly) where and when I want the gripper to be to intercept the object.

--patrick

[Adam Leeper]

+1 to Patrick's idea; I think being able to specify the duration/time would be very powerful. 

I guess the question would be, if the specified duration is too short, should it immediately abort, or go as fast as possible?

--Adam

[Mrinal Kalakrishnan]

On Thu, Jun 20, 2013 at 11:10 PM, Adam Leeper <ale...@willowgarage.com> wrote:
> +1 to Patrick's idea; I think being able to specify the duration/time would
> be very powerful.

+1 - I like the idea of specifying the time as well: in fact, I miss
this field which used to exist in arm_navigation.

> I guess the question would be, if the specified duration is too short,
> should it immediately abort, or go as fast as possible?

My preferred semantic would be that it's just a "preferred" or
"recommended" duration. If it's not feasible given the robot's
velocity limits, it should still be executed as fast as possible. An
application where the provided time is really critical (like
intercepting a moving object) will probably also have some customized
filters or adapters which can abort the trajectory if it's going to be
too slow.

- Mrinal

[Patrick Goebel]

Agreed!

--patrick

[Sam Pfeiffer]

Reviving this a bit!

Is there any implement solution? The idea of specifying the time for a goal fits exactly my current needs!

Thanks :)

[Ioan Sucan]

Just so we do not forget about this, can we have a summary of this feature request in a ticket on moveit_ros?

[Sam Pfeiffer]

Done: https://github.com/ros-planning/moveit_ros/issues/368

I've just asked for the field we had in move_arm for specifying a time for the plan. If anyone feels that is too vague or would like some more rich feature, please add content to the issue.

[robert.walenta]

Hi Everyone,

is there a way to use the scale method described by patrick with the move_group_action. I build up a Statemachine with it and want to trigger a velocity reduction on a sensor value. Is this possible with the move_group_action or do i have to go over the request of the execute() method?

Thanks for any ideas,
Robert