Velocity Cartesian Controller strange behavior

asked 2020-02-17 04:27:25 -0500

thanasis_t gravatar image

updated 2022-05-23 09:18:15 -0500

lucasw gravatar image


I am using a cartesian velocity controller for a robotic manipulator (UR3). The main functionality of the controller is that it accepts linear and angular velocities for the end effector. Then using IK it computes the joint velocities and updates the joint positions based on the joint velocities and the current joint positions. In the next example, the goal is for the end effector to track the green trajectory as shown in the image below. The orientation of the end effector remains constant while the linear velocities are computed based on waypoints along the trajectory. The problem is that the robot exhibits a strange behavior as shown in this gif. By running several tests I concluded that the manipulator has the tendency to lean towards its right when given velocities in a specific axis (y axis in this case), as shown in the end of the gif. Below the main functionality of the controller is shown, which is based on the Kinematics and Dynamics Library (KDL).

desired trajectory

for(std::size_t i=0; i < this->joint_handles_.size(); i++){
    this->joint_msr.q(i) = this->joint_handles_[i].getPosition();
    this->joint_msr.qdot(i) = this->joint_handles_[i].getVelocity();

ik_vel_solver->CartToJnt(this->joint_msr, x_vel_des, q_vel_cmd);

for(std::size_t i=0; i < this->joint_handles_.size(); i++) {
    this->joint_handles_[i].setCommand(this->joint_msr.q(i) + q_vel_cmd_(i)*time.toSec());

fk_vel_solver_->JntToCart(this->joint_msr, x_vel);
fk_pos_solver_->JntToCart(this->joint_msr.q, x);

Does anybody know the potential reason behind this strange behavior? Is it due to incorrect gravity modelling or could there be some other issue? The system is build in ROS Melodic, Ubuntu 18.04 and Gazebo 9.0. Thanks in advance and please let me know if there is any other information I could provide.

edit retag flag offensive close merge delete


Please attach your image and gif directly to the question.

Also: have you resolved #q339893?


this->joint_handles_[i].setCommand(this->joint_msr.q(i) + q_vel_cmd_(i)*time.toSec());

this looks like it is integrating velocity to get a position and then commanding that. That would seem to contradict:

The main functionality of the controller is that it accepts linear and angular velocities for the end effector and it produces joint velocities using IK which are passed to ros controllers.

gvdhoorn gravatar image gvdhoorn  ( 2020-02-17 04:45:40 -0500 )edit

I attached the image but couldn't attach the gif. As for your second question, no unfortunately I have not resolved it yet.

Indeed the description of the functionality of the controller was incorrect, so I modified it as well. Thanks for your comments.

thanasis_t gravatar image thanasis_t  ( 2020-02-17 07:03:57 -0500 )edit