Controlling velocity during execution of (Follow)JointTrajectory goals

[Gloriabhsfer]

Hi all:

Thanks for the VAL 3 Driver for staubli robot.

The last Thursday, since we have worked together with @marshallpowell97 and tested the example code in motoman_driver:motoman_driver/src/move_to_joint.py, it can realize some basic position control.

But the velocity cannot be controlled while using FollowJointTrajectory msg with some specific joint angles inside it. The velocity at the desired velocity(default is 100%)when we gave the group of angles and let the robot move. Usually, the ROS controller will calculate the time_form_start and using duration time to control the speed when doing the position control and define the velocity at the same time. But as you mentioned in the e-mail, the VAL 3 driver has ignored the time_form_start so the duration time cannot be assigned when using the FollowJointTrajectory function.

Does anyone get thoughts about how to implement this feature in the VAL3 driver?

Best,
Gloria

[gavanderhoorn]

But as you mentioned in the e-mail, the VAL 3 driver has ignored the time_form_start so the duration time cannot be assigned when using the FollowJointTrajectory function.

I'm assuming "you" here is @marshallpowell97?

Could you please clarify how you concluded "the VAL 3 driver [..] ignored the time_from_start"?

Because afaik, that is not the case.

time_from_start is converted into duration and velocity by the industrial_robot_client. It's then encoded into the JointTrajectory messages and decoded by the driver here:

staubli_val3_driver/staubli_val3_driver/val3/ros_server/pushMotion.pgx

Lines 42 to 43 in 1280ba7

	l_mTrajPt.mDesc.vel= limit((rosTrajPtMsg.jointTrajPt.nVelocity * 100), 0, 100)
	l_mTrajPt.nDuration=rosTrajPtMsg.jointTrajPt.nDuration

it could very well be that this leads to less than optimal motions, but it's certainly not "being ignored".

[Gloriabhsfer]

Yes, what I mentioned the e-mail is Mr.Powell, we have meet last week and tried this driver on our robot.

We discussed that because we lack the time_form_start parameter so we cannot control the duration time during the start and the end positions we gave for the robot. But it seems in this driver it's already been implement inside the controller.
We have tested the position control package for Motoman, with the move_to_joint.py code

python move_to_joint.py [0,0,0,0,0,0] 6

Where" 6 "represents the duration time, but the robot is still moving with the full of its velocity.

I am a little bit confused about the robot driver for each robot. we always add:

import roslib; roslib.load_manifest('XXX_driver') Where XXX represents the robot you have.

When we test the code written for the Motoman robot, I think that won't work because the driver we have is not for staubli robot, but it can work with the position control, as it uses FollowJointTrajectory which is msg-type for ros_controller. But the duration time has been ignored for some reason (or our format is incorrect).

In my understanding, if we want to write a python script just like https://github.com/ros-industrial/motoman/blob/kinetic-devel/motoman_driver/src/move_to_joint.py or https://github.com/ros-industrial/universal_robot/blob/kinetic-devel/ur_driver/test_move.py we need to import the roslib for staubli, which in this case is staubli_val3_driver, and write the code base on trajectory msgs type. Please correct me if my understanding is wrong.

Thanks for your reply

[gavanderhoorn]

Yes, what I mentioned the e-mail is Mr.Powell, we have meet last week and tried this driver on our robot.

Could I ask you (and @Bdelspi) to please not refer to private conversations you've had? There is no way for us to know what you've discussed, and as such, we cannot know what you are referring to.

Your OP was missing quite some details, which you've only added after I asked about it.

We discussed that because we lack the time_form_start parameter so we cannot control the duration time during the start and the end positions we gave for the robot. But it seems in this driver it's already been implement inside the controller.

This sounds like you're submitting JointTrajectorys which you haven't properly time-parameterised.

If that is the case, the problem would not be with the driver, but with how you generate your trajectories.

We have tested the position control package for Motoman, with the move_to_joint.py code

python move_to_joint.py [0,0,0,0,0,0] 6

Where" 6 "represents the duration time, but the robot is still moving with the full of its velocity.

Please do not use that script.

Refer to ros-industrial/motoman#229 for something better. But note: there are some parts in there which are Motoman-specific. Additionally: it uses a manual approach to set/calculate time_from_start, which is not what I would recommend.

I am a little bit confused about the robot driver for each robot. we always add:

import roslib; roslib.load_manifest('XXX_driver') Where XXX represents the robot you have.

You should never do that. It's not needed.

roslib.load_manifest(..) has been deprecated for quite a number of years now. See also q115346 on ROS Answers.

When we test the code written for the Motoman robot, I think that won't work because the driver we have is not for staubli robot, but it can work with the position control, as it uses FollowJointTrajectory which is msg-type for ros_controller. But the duration time has been ignored for some reason (or our format is incorrect).

I'm not sure what you mean by this.

Please provide sample code which shows the problem you are encountering.

In my understanding, if we want to write a python script just like https://github.com/ros-industrial/motoman/blob/kinetic-devel/motoman_driver/src/move_to_joint.py or https://github.com/ros-industrial/universal_robot/blob/kinetic-devel/ur_driver/test_move.py we need to import the roslib for staubli, which in this case is staubli_val3_driver, and write the code base on trajectory msgs type. Please correct me if my understanding is wrong.

Don't use either of those scripts.

As a high-level overview: all ROS-I drivers (and many other ROS drivers for (serial) manipulators) expose a FollowJointTrajectory action interface. This accepts goals which contain a JointTrajectory.

It is your responsibility to make sure those trajectories are properly configured, sufficiently smooth and adhere to the limits of the robot you're targetting.

People just starting our typically use MoveIt to generate trajectories that fulfill (most of) these requirements, but there are certainly other tools and libraries, as well as manual ways to work with them.

If you want to generate trajectories in some other way, be sure to create them properly. You could take a look at the Time Parameterization page in MoveIt's tutorials for some background information. Those algorithms may be used stand-alone as well.

Refer to ros-industrial/motoman#229 for an example script which shows the basics of setting up a FollowJointTrajectory action goal.

[gavanderhoorn]

I've updated the title of this issue as I don't believe we're looking to "implement time_from_start" in the driver, but you're actually asking how to generate trajectories which will result in certain velocities of the robot's joints during execution.

[Gloriabhsfer]

Hi Dr. Gavanderhoorn:

Thanks for your quick reply, I apologize to include the private conversation in this issue.
Yes,thanks for the demo code you mentioned in #229.

As a high-level overview: all ROS-I drivers (and many other ROS drivers for (serial) manipulators) expose a FollowJointTrajectory action interface. This accepts goals which contain a JointTrajectory.

Yes, I think that's answered my question: other robot have the robot_driver but staubli robot's driver is for VAL3 language.

It is your responsibility to make sure those trajectories are properly configured, sufficiently smooth and adhere to the limits of the robot you're targetting.
People just starting our typically use MoveIt to generate trajectories that fulfill (most of) these requirements, but there are certainly other tools and libraries, as well as manual ways to work with them.
If you want to generate trajectories in some other way, be sure to create them properly. You could take a look at the Time Parameterization page in MoveIt's tutorials for some background information. Those algorithms may be used stand-alone as well.
Refer to ros-industrial/motoman#229 for an example script which shows the basics of setting up a FollowJointTrajectory action goal.

Agree, and we would like to included the motion planning part in their controller. In order to test the ROS connection between PC and robot, we just give some safe waypoint as our first step. We will using MoveIt or other method to calculate the IK result and planning the trajectory.
Please correct me if I am wrong, so the FollowJointTrajectory action interface is applicable for all robot using ROS. We can use MoveIt mode to generate a safe and properly trajectory and sent them to the rostopic we control the robot. To define the duration time, I need to look at the Time Parameteriztion page in MoveIt and create the MoveIt package using python interface or C++ interface.

Best,
Gloria

[gavanderhoorn]

I'm not entirely sure I completely understand what you write in your comment.

As a summary: FollowJointTrajectory is a typically offered, high-level interface which offers motion control at trajectory granularity. It is not really intended for low-level motion control of ROS enabled robots.

In general, for this trajectory interface to work properly, you'll need to make sure to populate all fields correctly. Continuity of motion cannot be guaranteed unless the input trajectory is smooth, and all relevant fields have appropriate values (ie: velocity and acceleration).

In the case of staubli_val3_driver specifically though, the interface is such that any JointTrajectory is pre-processed to reduce information like velocities and accelerations down to what the staubli controller accepts (ie: single nrs for velocity/motion duration and acceleration). staubli_val3_driver does not implement a high-bandwidth, real-time / on-line external motion interface, where this would be different.

There are certainly ways to implement such an interface with ROS, but the feasibility of doing this would also depend on what Stäubli controllers support.

[gavanderhoorn]

As I believe the issues raised here are not really issues with the code in this repository, but are questions about the usage of the generic, ROS-based, FollowJointTrajectory interface, I'm closing this issue.

It's perfectly fine of course to keep commenting on it.

[marshallpowell97]

Just to follow up here,

time_from_start is converted into duration and velocity by the industrial_robot_client

I am not 100% sure if this is getting converted correctly. I ran this script (only changing the joint names) and see the following on the /joint_trajectory_action/goal rostopic:

header: 
  seq: 1
  stamp: 
    secs: 1600198254
    nsecs:  41970014
  frame_id: ''
goal_id: 
  stamp: 
    secs: 1600198254
    nsecs:  41940927
  id: "/simple_trajectory_action_client-1-1600198254.042"
goal: 
  trajectory: 
    header: 
      seq: 0
      stamp: 
        secs: 0
        nsecs:         0
      frame_id: ''
    joint_names: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6]
    points: 
      - 
        positions: [-1.3962634511699434e-05, 0.0, 0.0, 0.0, 0.0, -2.09439513128018e-05]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 0
          nsecs:         0
      - 
        positions: [-1.3962634511699434e-05, 0.0, 0.0, 0.0, 0.0, -0.3490867943501787]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 5
          nsecs:         0
      - 
        positions: [-1.3962634511699434e-05, 0.0, 0.0, 0.0, 0.0, -2.09439513128018e-05]
        velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        accelerations: []
        effort: []
        time_from_start: 
          secs: 10
          nsecs:         0
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance: 
    secs: 0
    nsecs:         0
---

I ran this while debugging on the Staubli controller, and the velocity received on each point was 0.1 (which is what I would expect when sending a velocity of 0.) Is it possible that industrial_robot_client is only converting the desired velocity at the point and not to the point? @gavanderhoorn let me know if I'm way off base here.

[gavanderhoorn]

Is it possible that industrial_robot_client is only converting the desired velocity at the point and not to the point?

No, that's correct.

But this is not specific to industrial_robot_client per se: in ROS/MoveIt (and with other (planning) frameworks), trajectories encode the desired state at a point in time. Industrial robots on the other hand typically only support encoding a motion to a destination. Associated with this motion is then a set of modifiers which encode constraints on how that motion should be executed.

This is a fundamental difference, which the IRC tries to work-around.

One way it does that is by converting delta-t between two states to segment length (which, if you take the inverse, is roughly equivalent to velocity).

Of course this is only a rough approximation, as it typically can't control acceleration or any other parameters (or modifiers).

That's why for robots which have better drivers and/or external motion interfaces, IRC is never used. It's a relatively primitive way to interface with motion controllers, and was created out of necessity.

[marshallpowell97]

That makes sense to me. So in this case, since time_from_start has a value of 5 and 10 for the second and third points in the trajectory, shouldn't the velocity received by the Staubli controller at each of those points be something other than 0.1? Even with a different value for time_from_start, it doesn't seem to have any effect (velocity stays at 0.1).