Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Velocity streaming #218

Closed
miguelprada opened this issue Oct 23, 2018 · 37 comments
Closed

Velocity streaming #218

miguelprada opened this issue Oct 23, 2018 · 37 comments
Labels
kinetic Issues with the refactor in Kinetic

Comments

@miguelprada
Copy link
Contributor

miguelprada commented Oct 23, 2018

This is a follow up to a discussion started by this comment by @AndyZe.

The refactor of this driver got rid of the /joint_speed topic functionality, which could be used to stream joint velocity commands to the robot. It is an open question whether that topic should be restored, or whether similar enough functionality can be achieved by either:

  • streaming positions to the default velocity_controllers/JointTrajectoryController
  • using a velocity_controllers/JointGroupVelocityController

Ping: @AndyZe, @gavanderhoorn

@gavanderhoorn
Copy link
Member

The refactor of this driver got rid of the /joint_speed topic functionality

This phrasing makes it sound like it was a deliberate choice to remove it.

That was / is not the case. It seems this was more of an oversight.

It is an open question whether that topic should be restored, or whether similar enough functionality can be achieved by either:

* streaming positions to the default `velocity_controllers/JointTrajectoryController`
* using a `velocity_controllers/JointGroupVelocityController`

It makes quite a bit of sense to me to add support for joint_speed back to the kinetic-devel version again.

I like using standardised interfaces, but even just for bw-compatibility the topic (and underlying infrastructure) should be added back in.

Using velocity_controllers/JointGroupVelocityController might be a good option for a future enhancement, but would also require the addition of joint velocity handles in the underlying hardware_interface.


PS: not sure what is going on with GH still: I seem to be missing a lot of notification emails.

@miguelprada
Copy link
Contributor Author

The refactor of this driver got rid of the /joint_speed topic functionality

This phrasing makes it sound like it was a deliberate choice to remove it.

That was / is not the case. It seems this was more of an oversight.

My apologies, it wasn't my intention to imply that.

It makes quite a bit of sense to me to add support for joint_speed back to the kinetic-devel version again.

I like using standardised interfaces, but even just for bw-compatibility the topic (and underlying infrastructure) should be added back in.

I'm not a fan of adding the topic back, since this driver is already a bit much of a mix of different options for my taste, although backwards compatibility is definitely a big point in favor.

Using velocity_controllers/JointGroupVelocityController might be a good option for a future enhancement, but would also require the addition of joint velocity handles in the underlying hardware_interface.

This driver has always provided joint velocity handles. In fact, the default spawned instance of the joint_trajectory_controller makes use of them.

@gavanderhoorn
Copy link
Member

Using velocity_controllers/JointGroupVelocityController might be a good option for a future enhancement, but would also require the addition of joint velocity handles in the underlying hardware_interface.

This driver has always provided joint velocity handles. In fact, the default spawned instance of the joint_trajectory_controller makes use of them.

you're correct. I got my drivers mixed up :S

@atenpas
Copy link

atenpas commented Oct 29, 2018

I'm a fan of adding the topic back because our custom velocity controller relies on it. AFAIK, JointTrajectoryPoint messages need to be timed and can thus not be used to stream velocities.

@AndyZe
Copy link

AndyZe commented Oct 29, 2018

I believe a ros::Time(0.0) in the header of a JointTrajectoryPoint means "execute now," so that might work. I haven't had time to try it yet.

@miguelprada
Copy link
Contributor Author

I believe a ros::Time(0.0) in the header of a JointTrajectoryPoint means "execute now," so that might work. I haven't had time to try it yet.

I believe you're right, yes. But that's only regarding the timestamp in the JointTrajectory message. Every JointTrajectoryPoint in the trajectory carries an additional time_from_start field that I believe must contain some non-zero value not to be dropped by the JointTrajectoryController. You can probably use the JointTrajectoryController to emulate streaming velocities, by integrating the desired velocities into positions and setting both positions and time_from_start appropriately.

I would, however, avoid this in favor of either using JointGroupVelocityController or restoring the original behavior of the custom joint_speed handler in this driver.

@atenpas, do you use this custom controller with anything other than this driver? Is there any other robot that implements joint speed streaming (ab)using JointTrajectory messages in a similar way? Why not refactor it to use JointGroupVelocityController through Float64MultiArray messages?

@gavanderhoorn
Copy link
Member

Using JointTrajectory for velocity is abusing the message somewhat. I would not recommend it.

Assigning special semantics to messages with time_from_start also seems very brittle, as that is not documented anywhere afaik (apart from the time_from_start at the JointTrajectory level) and different action servers will implement this differently (or completely ignore it).

Using proper interfaces would have my preference, and the JointGroupVelocityController seems like a proper one. I do believe we should reintroduce the joint_speed topic (but I already wrote that earlier).

Is there any other robot that implements joint speed streaming (ab)using JointTrajectory messages in a similar way? Why not refactor it to use JointGroupVelocityController through Float64MultiArray messages?

off-topic here, but using Float64MultiArray is really ugly imo for this kind of task. There should be a proper message with appropriate semantics for velocity controlled joint(s) (groups). A bunch of float64s do not have any semantics, which is bad.

@miguelprada
Copy link
Contributor Author

Using JointTrajectory for velocity is abusing the message somewhat. I would not recommend it.
[...]
I do believe we should reintroduce the joint_speed topic (but I already wrote that earlier).

Your first statement is one of the reasons I'm against reintroducing the joint_speed topic. Not only it's semantically incorrect, but it also makes a quite particular use of the contents in JointTrajectory message.

off-topic here, but using Float64MultiArray is really ugly imo for this kind of task. There should be a proper message with appropriate semantics for velocity controlled joint(s) (groups). A bunch of float64s do not have any semantics, which is bad.

I totally agree, but I find it less ugly to use Float64MultiArray than JointTrajectory for this use-case.

@gavanderhoorn
Copy link
Member

Using JointTrajectory for velocity is abusing the message somewhat. I would not recommend it.
[...]
I do believe we should reintroduce the joint_speed topic (but I already wrote that earlier).

Your first statement is one of the reasons I'm against reintroducing the joint_speed topic. Not only it's semantically incorrect, but it also makes a quite particular use of the contents in JointTrajectory message.

true, but with the refactor we broke something that already worked for quite a few users.

Resolving that situation is the rationale for ignoring the abuse of JointTrajectory here.

@miguelprada
Copy link
Contributor Author

true, but with the refactor we broke something that already worked for quite a few users.

That's the only reason I see to reintroduce this functionality.

However, if no other driver provides a topic with the same semantics, it may be less effort to update client code rather than updating the driver. That's why I was asking @atenpas about it, I'd like to know how bad we want backwards compatibility with a questionable functionality.

Anyways, we're running in circles here. I guess the real measure of how bad this feature is needed will be to see whether anyone comes up with a PR restoring it 😉

@gavanderhoorn
Copy link
Member

Anyways, we're running in circles here. I guess the real measure of how bad this feature is needed will be to see whether anyone comes up with a PR restoring it wink

Agreed.

@atenpas
Copy link

atenpas commented Oct 30, 2018

@atenpas, do you use this custom controller with anything other than this driver? Is there any other robot that implements joint speed streaming (ab)using JointTrajectory messages in a similar way? Why not refactor it to use JointGroupVelocityController through Float64MultiArray messages?

I'm using it on all the UR arms we have (four ur5s, one ur10). However, for now, I'm just using an older commit that still has the topic available. Do you have an example for using JointGroupVelocityController?

@atenpas
Copy link

atenpas commented Oct 30, 2018

Using proper interfaces would have my preference, and the JointGroupVelocityController seems like a proper one. I do believe we should reintroduce the joint_speed topic (but I already wrote that earlier).

Is there any other robot that implements joint speed streaming (ab)using JointTrajectory messages in a similar way? Why not refactor it to use JointGroupVelocityController through Float64MultiArray messages?

off-topic here, but using Float64MultiArray is really ugly imo for this kind of task. There should be a proper message with appropriate semantics for velocity controlled joint(s) (groups). A bunch of float64s do not have any semantics, which is bad.

This discussion is somewhat confusing to me. Is JointGroupVelocityController the proper interface to stream velocities or is a separate topic (e.g., joint_speed) necessary?

@AndyZe
Copy link

AndyZe commented Oct 30, 2018

I think the consensus is yes, JointGroupVelocityController is the best way. It is more compatible with other robots. /joint_speed was just a secondary, convenient option.

But a JointGroupVelocityController has not been implemented for UR robots yet. I'm working on a PR now.

@miguelprada @gavanderhoorn Can you recommend where the code for a JointGroupVelocityController should go? I'm grepping through both ur_modern_driver and universal_robot, but I don't see where the controllers are implemented.

The current error is:
Unknown controller type: JointGroupVelocityController

This occurs when launching the move_group node. It looks like ur_modern_driver loads the JointGroupVelocityController just fine.

@miguelprada
Copy link
Contributor Author

@miguelprada @gavanderhoorn Can you recommend where the code for a JointGroupVelocityController should go? I'm grepping through both ur_modern_driver and universal_robot, but I don't see where the controllers are implemented.

There's nothing to implement. You should only need to add the configuration below to the controller config files (e.g. ur3_controllers.yaml).

joint_speed:
  type: velocity_controllers/JointGroupVelocityController
  joints:
    - shoulder_pan_joint
    - shoulder_lift_joint
    - elbow_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint

And possibly add the controller to the list of loaded controllers, e.g. here.

Not sure joint_speed is the right name, though, since it can lead to confusion. Maybe joint_velocity_controller?

@miguelprada
Copy link
Contributor Author

Oops, and I forgot. An exec_depend should also be added on velocity_controllers.

@AndyZe
Copy link

AndyZe commented Oct 30, 2018

Well, I didn't need to add an exec_depend but it randomly started working. ¯_(ツ)_/¯

@miguelprada
Copy link
Contributor Author

You don't need the exec_depend for this to work, as long as you have that dependency on your system (either installed or in your workspace). However, this is needed for tools such as rosdep and the ROS buildfarm to correctly determine the package's dependencies.

@gavanderhoorn
Copy link
Member

With #224 merged the kinetic-devel branch now has support for JointGroupVelocityControllers. At least when using the ros_control configuration of the driver.

I still think adding back the joint_speed topic would be a good idea, if only because it can be used with the non-ros_control side of things.

@miguelprada miguelprada added the kinetic Issues with the refactor in Kinetic label Nov 9, 2018
@Chunting
Copy link

Chunting commented Apr 22, 2019

Hi,
I've read all your comments, but still confused about how to implement velocity control.
Now I'm using moveit to do the path planning in ros_control mode (vel_based_pos_traj_controller).

std::string right_controller_ = "right/right_vel_based_pos_traj_controller";
moveit_simple_controller_manager::FollowJointTrajectoryControllerHandle handle_right(right_controller_, "follow_joint_trajectory");
 if (plan_right.trajectory_.joint_trajectory.joint_names.size() > 0)
 {
        ROS_INFO("Trajectory sent to right arm");
        dual_arm_toolbox::TrajectoryProcessor::visualizePlan(plan_right, 0);
        success_right = handle_right.sendTrajectory(plan_right.trajectory_);
  }
 if (plan_right.trajectory_.joint_trajectory.joint_names.size() > 0)
 {
        success_right = handle_right.waitForExecution();
 }

Would you please give me some tips about how to change it to velocity control?
Or provide further information about how to use JointGroupVelocityControllers?

I will appreciate your kind help.

Thanks,
Chunting

@gavanderhoorn
Copy link
Member

@Chunting: MoveIt is kinematic planning only. It does not "plan velocities".

This issue is about streaming velocity commands (ie: requesting that joint_X attain a velocity of Y rad/s).

If you're asking how to configure the driver such that it uses the velocity-based position controllers that would be something else.

@Chunting
Copy link

Hi @gavanderhoorn
Thanks for your quick reply.
My question is about streaming velocity commands.
I need to add the following contents in ur5_controller.yaml

right_joint_group_vel_controller:
   type: velocity_controllers/JointGroupVelocityController
   joints:
     - right_shoulder_pan_joint
     - right_shoulder_lift_joint
     - right_elbow_joint
     - right_wrist_1_joint
     - right_wrist_2_joint
     - right_wrist_3_joint

However, how to use it in my C++ side?
I just use moveit to get the joint velocity, certainly, a better way is to use jacobian matrix to do IK instead of moveit.

But I've no idea how to send the joint velocity command to your driver in ros control mode?

Is this the right way to call "joint_group_vel_controller"?

std::string right_controller_ = "right/right_joint_group_vel_controller";
moveit_simple_controller_manager::FollowJointTrajectoryControllerHandle handle_right(right_controller_, "follow_joint_trajectory");
 if (plan_right.trajectory_.joint_trajectory.joint_names.size() > 0)
 {
        ROS_INFO("Trajectory sent to right arm");
        dual_arm_toolbox::TrajectoryProcessor::visualizePlan(plan_right, 0);
        success_right = handle_right.sendTrajectory(plan_right.trajectory_);
  }
 if (plan_right.trajectory_.joint_trajectory.joint_names.size() > 0)
 {
        success_right = handle_right.waitForExecution();
 }

Thanks,
Chunting

@gavanderhoorn
Copy link
Member

gavanderhoorn commented Apr 22, 2019

I just use moveit to get the joint velocity, certainly, a better way is to use jacobian matrix to do IK instead of moveit.

Yes, using MoveIt for that does not make much sense to me. The velocities you get from a JointTrajectory are not going to be sufficient for the robot to actually reach the positions.

But I've no idea how to send the joint velocity command to your driver in ros control mode?

JointGroupVelocityController accepts Float64MultiArray messages on a command topic where each message contains a 1D array of N floats where N is the number of joints in the group.

So no: you cannot use a moveit_simple_controller_manager::FollowJointTrajectoryControllerHandle for that. Actionlib is actually not involved in this at all.

See here for where the subscriber is created.

We're all happy to help of course, but this is really a question for the ros_control maintainers. We're users of the JointGroupVelocityController, just as you are.

@Chunting
Copy link

Thanks for your kind help.
I do not care about the ros control thing, but just wanna implement velocity control in my project.
In my project, I used ros control to do the position control and joint_speed topic to do the velocity control, but you removed it in the new driver.
Thus, my question is about how to implement velocity control to replace joint_speed topic.
Also, I recommend you to provide an example in the test case, I think it is very helpful.

Now I accept your suggestion and try the command topic.
Thanks again for your patience and help, I really appreciate it.

Chunting

@gavanderhoorn
Copy link
Member

I do not care about the ros control thing, but just wanna implement velocity control in my project.

the "ros control thing" is essentially just a way to get the JointGroupVelocityController running which will let you stream velocities. Which seems like it is what you want to do ..

In my project, I used ros control to do the position control and joint_speed topic to do the velocity control,

so by using JointGroupVelocityController you wouldn't need to use two different approaches any more.

but you removed it in the new driver.

as I already wrote in #218 (comment), it was not a deliberate choice to "remove it", but an oversight in the Kinetic refactor.

As I also wrote (in #218 (comment)), the joint_speed topic is abusing JointTrajectory messages and that should not be perpetuated. Using JointGroupVelocityController is better and should result in the exact same behaviour of the robot (the exact same URScript interfaces are used).

Thus, my question is about how to implement velocity control to replace joint_speed topic.

That should have been answered by my earlier comment.

If you really want joint_speed back, then it would be great if you could submit a pull request porting it from master to kinetic-devel. For bw-compatibility reasons we'd accept that, however keep in mind that JointGroupVelocityController is the preferred way of streaming joint velocities to the controller.

@Tina1994
Copy link

Tina1994 commented May 31, 2019

@gavanderhoorn Hello, I have some question about using JointGroupVelocityController, I want to use velocity controllers ros-control to control the ur10 robot, and I modified the controller.yaml in the ur10_moveit_config as you can mentioned in #224, as you can see in the following:

controller_list:
  - name: joint_group_vel_controller
    type: velocity_controllers//JointGroupVelocityController
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint

And I use the ur10_ros_control.launch to connect the robot, however, when input : roslaunch ur10_moveit_config ur10_moveit_planning_execution.launch, I meet the error:

[ WARN] [1559273539.075373221]: Please note that 'action_ns' no longer has a default value.
[ERROR] [1559273539.075442276]: Unknown controller type: /JointGroupVelocityController

How could I fix it? thanks very much.

@gavanderhoorn
Copy link
Member

gavanderhoorn commented May 31, 2019

I want to use velocity controllers ros-control to control the ur10 robot,

with ur_modern_driver?

and I modified the controller.yaml in the ur10_moveit_config

that's a MoveIt file, it has no connection to the driver or configuring the JointGroupVelocityController.

And I use the ur10_ros_control.launch to connect the robot, however, when input :

roslaunch ur10_moveit_config ur10_moveit_planning_execution.launch

I meet the error:

[ WARN] [1559273539.075373221]: Please note that 'action_ns' no longer has a default value.
[ERROR] [1559273539.075442276]: Unknown controller type: /JointGroupVelocityController

which is MoveIt telling you that it doesn't know anything about a JointGroupVelocityController and it is correct: that is not a MoveIt controller type.

You don't need to change anything in ur_modern_driver to use JointGroupVelocityController with ur_modern_driver: the controller is already configured here:

https://github.com/ros-industrial/ur_modern_driver/blob/6b818fa149bcd070131d6b04e5d97e2f67d28684/config/ur10_controllers.yaml#L98-L107

You need to make sure to start the driver in ros_control mode and then use the controller manager to switch to the joint_group_vel_controller.

I want to use velocity controllers ros-control to control the ur10 robot, and I modified the controller.yaml in the ur10_moveit_config as you can mentioned in #224,

Please note: what is described in the readme (not the PR) is how to use MoveIt with the velocity-based JointTrajectoryController, which is still a position controller.

MoveIt cannot use a JointGroupVelocityController.

@Tina1994
Copy link

@gavanderhoorn Thanks for you help. I did not change the ur10_controller.yaml. I only change the controller.yaml in the ur10_moveit_config folder. I'll try to publish the FLoatMultiarray msgs to see how it works. Thanks you again!

@Tina1994
Copy link

Tina1994 commented May 31, 2019

@gavanderhoorn Hi, I am still having some problems with the use of /joint_group_vel_controllers. I try to pub some messages to the ur10 through FLoatMultiarray msgs like the following:

rostopic pub -1 /joint_group_vel_controller/command std_msgs/Float64MultiArray "layout: 
  dim:
  - label: ''
    size: 0
    stride: 0
  data_offset: 0
data: [0.0, 0.0,0.0,0.0,0.0,1.0]"

But the robot last joint still can not move,could you help me to solve it ?
Thanks very much.

@AndyZe
Copy link

AndyZe commented May 31, 2019

Hi @Tina1994,

Two things to try:

Try a much smaller velocity, like:
data: [0.0, 0.0,0.0,0.0,0.0,0.001]

We don't want a sudden jump.

Also, make sure the joint_group_vel_controller is the active controller with:
rosservice call /controller_manager/list_controllers

If the joint_group_vel_controller is not active, use rosservice call /controller_manager/switch_controllers

http://docs.ros.org/api/controller_manager_msgs/html/srv/SwitchController.html

Hopefully the next generation of ros_control won't need switching like this.

@gavanderhoorn
Copy link
Member

gavanderhoorn commented May 31, 2019

@AndyZe wrote:

Hi @Tina1994, can you ask these questions at the jog_arm repo?

I don't see @Tina1994 mentioning jog_arm anywhere?

Try a much smaller change in position, like:
data: [0.0, 0.0,0.0,0.0,0.0,0.001]

isn't @Tina1994 trying to use velocity control?

@AndyZe
Copy link

AndyZe commented May 31, 2019

My bad, you're right. (edited)

@AndyZe
Copy link

AndyZe commented May 31, 2019

I was confused by the similar question here: UTNuclearRoboticsPublic/jog_arm#94

@gavanderhoorn
Copy link
Member

I was confused by the similar question here: UTNuclearRoboticsPublic/jog_arm#94

understandable.

@gavanderhoorn gavanderhoorn added the wrid19 World ROS-Industrial Day 2019 label Jun 29, 2019
@gavanderhoorn gavanderhoorn removed the wrid19 World ROS-Industrial Day 2019 label Jul 17, 2019
@gavanderhoorn
Copy link
Member

I'm closing this as we've officially deprecated this package.

Refer to the announcement on ROS Discourse.

@gavanderhoorn
Copy link
Member

Velocity streaming is supported by ur_modern_driver in UniversalRobots/Universal_Robots_ROS_Driver#1.

This will be merged with ur_robot_driver/master in the (near) future.

Community contributions and help would be appreciated.

@fmauch
Copy link

fmauch commented Nov 25, 2019

Velocity streaming is supported by ur_modern_driver in UniversalRobots/Universal_Robots_ROS_Driver#1.

This will be merged with ur_robot_driver/master in the (near) future.

Community contributions and help would be appreciated.

Small correction of the above:
Velocity streaming is supported by ur_robot_driver

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
kinetic Issues with the refactor in Kinetic
Projects
None yet
Development

No branches or pull requests

7 participants