Velocity Cartesian Controller strange behavior

edit

Hello,

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).

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.