vel_based_pos_traj_controller depends on positions and time_from_start rather than velocities only

[haisongdong-harrison]

I need to do some visual servoing task with ViSP, and it will generate velocity command for all joints so I just switched to ros_control to use vel_based_pos_traj_controller.

To my understanding, it should only take the velocities in the command I published to topic /vel_based_pos_traj_controller/command because that's all you need to call the underlying setSpeed() function. It seems to me that, in a sense, it works almost in the same way as the /ur_driver/joint_speed topic except that the controller will also do some PID control for the final output.

However, our UR3 behaves very differently from what we expected with this vel_based_pos_traj_controller. Firstly, if we only specify velocities in the message, the driver complains something like "size mismatch" for positions, accelerations and other fields. But if we fill in these fields with some dummy values, the robot moves in velocities that are more like calculated from the positions and time_from_start, rather than the velocities we supplied. When we keep everything else the same, a small time_from_start will lead to a really fast motion of UR3 while a great value slows it down a lot.

It's my first time using ros_control and this controller, so I'm not sure if I'm doing something wrong here. Any help would be greatly appreciated. Thanks!

[HurchelYoung]

Hello hansondon, You have probably already figured this out, but since this is still open...

The difference between the /vel_based_pos_traj_controller and the /pos_based_pos_traj_controller is in the type of commands that are sent to the robot:

• The velocity based controller sends joint speed commands to the robot, using the speedj command
• The position based controller sends joint position commands to the robot, using the servoj command

They both interpret the joint plan in exactly the same way as you have observed. ThomasTimm suggests the following for visual servoing:

/joint_speed : Takes messages of type trajectory_msgs/JointTrajectory. Parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint_names, so set the values in the correct order.

[haisongdong-harrison]

Hi HurchelYoung, Thanks for the comment! In fact, this is also what we figured out to do at the end. The reason why we asked the question in the first place was that both /vel_based_pos_traj_controller and /joint_speed topic uses speedj command under the hood so we expected them to both take nothing but velocities as inputs, but it isn't true.

As a side note, for anyone who also happens to work on visual servoing tasks with ur_modern_driver, we experienced a lot of oscillations and protective stops when using the driver as is. And we found that there are at least three (hacky) things you can do to fix the problems:

1. In our case, we use visp to calculate the velocities for all the joints. We noticed that the servo task is relatively computation-intensive so the frequency at which our visp node publishes the velocities can never reach 125Hz. In the end, we have to fix the rate of the servo task loop at 62.5Hz and change the time argument of speedj command inside setSpeed() function from 0.008 to 0.016(1/62.5) or even 0.032. This is different from what other people reported (e.g. #92, #100 suggested to use 0.008 for better performance) but it greatly alleviates the oscillation problem. Our conclusion is that it is not always good to use a small time argument as it also has to match how fast you can generate the command. Update: this turned out to be a problem with the built-in CurrentStateMonitor in MoveIt!, which waits for the next joint state update and introduces a delay of 0.008s. Defining your own CurrentStateMonitorPtr then getCurrentState() can solve the problem.
2. The ros rate 62.5Hz 125Hz isn't a hard frequency limit so occasionally it takes longer than 0.016s 0.008s for the servo task loop to calculate a legit velocity command. This may cause the safety_count_ variable to increase and ur_modern_driver will use it to stop the robot if no command is received for a certain period of time. However, the default value for safety_count_max is set to 300, which is too great even for the original time argument 0.008 as it won't be triggered until 2.4s later. So before the driver can send any command to stop the robot, it's likely that some protective stop event has already occurred on the UR side. Since we are using a large time argument for now, we set the safety_count_max to 2.
3. I've read some posts in the UR community that it is preferable to use stopj rather than pass all zeroes to speedj to actually stop the robot. But now when safety_count_max_ is reached, the driver tries to stop the robot with setSpeed(0., 0., 0., 0., 0., 0.) that uses exactly speedj. We tested to pause the servo task while the robot is following some target at a relatively high velocity in order to trigger this safety stop, and we can replicate a protective stop almost every time. At a lower velocity, protective stop still happens but not as often. Then we replaced the setSpeed command with stopj and we got rid of all the strange protective stops completely. Now protective stop is only triggered when the robot really needs to stop (e.g. when there is a collision or some joint is close to its limit).

[Abduoit]

Hello folks @hansondon

I am also doing visual servoing with ViSP and ur5, but I am still stuck with applying pbvs or ibvs examples. All I did, I could publish the RQ-code's pose in ros and subscribe to it. BUT, I don't know how to apply this PBVS example ??, then publish the velocity to UR5 driver?? Would you please help with this??

This is the ROS node that subscribe to the published topic

#include <vector>

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"

std::vector<geometry_msgs::PoseStamped::ConstPtr> poses;

void handle_poses(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
  ROS_INFO_STREAM("Position X " << msg->pose.position.x);
  ROS_INFO_STREAM("Position Y: " << msg->pose.position.y);
  ROS_INFO_STREAM("Position Z: " << msg->pose.position.z);
  ROS_INFO_STREAM("Orientation X: " << msg->pose.orientation.x);
  ROS_INFO_STREAM("Orientation Y: " << msg->pose.orientation.y);
  ROS_INFO_STREAM("Orientation Z: " << msg->pose.orientation.z);
  ROS_INFO_STREAM("Orientation W: " << msg->pose.orientation.w);
  // Use the msg object here to access the pose elements,
  // like msg->pose.pose.position.x
  poses.push_back(msg);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listenerccp");

  ros::NodeHandle n;

  ros::Subscriber sub = n.subscribe("/visp_auto_tracker/object_position",
    1000, handle_poses);

  ros::spin();

  return 0;
}

[gavanderhoorn]

@Abduoit: I'm sorry to hear that you're having trouble getting visual servoing to work with Visp, but this is not the place to post these kind of questions. This is an issue tracker, and we should reserve it for actual issues.

I recommend you ask this on ROS Answers.

[gavanderhoorn]

@hansondon and @HurchelYoung: thanks for both your inputs and follow ups.

As you noticed, ros_control velocity based position controllers are still position controllers. They just use a velocity interface to close their own position loop over.

As this is not really an issue with this driver, I'll close the issue.

[Abduoit]

@hansondon would you please leave your comment on my my question at ROS answer. Thanks

[geweihgg]

Hi, @haisongdong-harrison , I'm using this vel_based_pos_traj_controller too, and I want to control the UR5 arm with FollowJointTrajectory. If I only fill in the velocities term, error occurs: "size mismatch in trajectory point position, velocity or acceleration data." Have you solve this problem? Thank you very much~ GeWei

[haisongdong-harrison]

Hi @geweihgg,

As @gavanderhoorn said above, vel_based_pos_traj_controller is still a position controller. In that case, velocity alone is not enough to define a trajectory point.

If you want to control the robot with velocity commands only, you should use the /ur_driver/joint_speed topic instead, which is what we did in the end.

[geweihgg]

Hi, @haisongdong-harrison , Do you know how to control UR5 with velocity command in Gazebo? I tried to define a velocity_controllers/JointTrajectoryController, but this controller provide us with a position interface like the ur_modern_driver. GeWei

[haisongdong-harrison]

@geweihgg We didn't use Gazebo, so I am not sure how exactly can you do that. But in order to test our velocity command in simulation mode, we send a position command that is computed as follows: current joint positions + joint velocities * inverse of control frequency (e.g. 0.008 at 125Hz) This computation ignores inertia and many other factors but is good enough for us. If you think this approximation is acceptable I guess you can do something similar.

[geweihgg]

@haisongdong-harrison Yes, I've thought about this approach, but I'm not sure about how it performs. How do you set the time_from_start field? 0.008? Thank you for your patience.

[haisongdong-harrison]

@geweihgg Well, we use the same control loop (runs at 125Hz) for both controlling the robot and simulation. So each iteration we only need to send a single-point trajectory and the time_from_start field does not matter to us because it will be executed immediately anyway. You may leave it as default(0).

[geweihgg]

@haisongdong-harrison But when I set time_from_start to 0.0, moveit.launch will WARN me like this: """WARN: Dropping all 1 trajectory point(s), as they occur before the current time. Last point is 0.000s in the past.""" And the UR5 arm doesn't move.

[haisongdong-harrison]

@geweihgg I am guessing that maybe you forgot to set the timestamp in command header to ros::Time::now().

[geweihgg]

@haisongdong-harrison , Ok, it works now. Thank you very much~