cartesian velocity control loop implementation

Question

I'm using ROS (noetic) to intuitively control a franka manipulator using the panda_robot package for the simulation.

I've set up an extended kalman filter which fuses the following measures:

• IMU data: the sensor is attached to my arm and reports the orientation of my hand wrt the (virtual) base link of the manipulator, along with the angular velocity
• position data coming from my mouse. The z coordinate is controlled through the mouse wheel
• linear velocity obtained by differencing the position of the mouse. As for the position data, the velocity is with respect to the (virtual) base link of the manipulator, which corresponds to the origin of the world in the Gazebo simulator.

The filter outputs an odometry message, which I'm trying to exploit to implement the following cartesian velocity control loop:

where $\dot{x}^*$ is the target velocity in cartesian space, while $x^*$ is the target position in the cartesian space. Both quantities are obtained from the kalman filter. Essentially, the filter acts as the reference generator depicted on the image, sending commands at about 100 Hz.

The code for the robot control loop is the following (using rospy):

    import rospy
    import numpy as np
    import tf
    ...


    def on_cmd(self, cmd):

      # cartesian speed and position obtained by the kalman filter
      tgt_pose = np.array(cmd.tgt_pose)
      tgt_twist = np.array(cmd.tgt_twist)
    
      # get current end-effector position and orientation in cartesian space
      ee_position, ee_orientation = self._panda_arm.ee_pose()
      ee_orientation = self._quat2array(ee_orientation)         # w,x,y,z --> x,y,z,w
      ee_orientation_rpy = tf.transformations.euler_from_quaternion(self._ee_orientation) 
      ee_pose = np.concatenate([ee_position, ee_orientation_rpy])
    
      # get the jacobian for the current joint configuration
      J = self._panda_arm.zero_jacobian()
      J_inv = np.linalg.pinv(J)
    
      pose_error = tgt_pose - ee_pose
    
      # self._K is a 6x6 diagonal positive-definite matrix
      twist_error = tgt_twist + self._K @ pose_error  
    
      joint_velocity_cmd = J_inv @ twist_error
    
      # finally execute the command
      self._panda_arm.exec_velocity_cmd(joint_velocity_cmd)

$K$ is chosen to be a 6x6 diagonal matrix with values of 0.01.

By means of rviz, I can see that the cartesian speed and velocity produced by the kalman filter seem to be correct. Still, I'm struggling to make the manipulator move accordingly to my mouse motion and my arm orientation. The robot is currently able to just move back and forth following the mouse, but it is basically ignoring the orientation. The result is that it appears to move randomly when I attempt to perform some more advanced moves (like rotating the gripper of the end effector). I've come to the assumption that the commands value provided by the filter are correct, so I think that there should be some error in my implementation of the control loop. I'd like to ensure that the above code snippet does make sense, at least conceptually.

Answer 1

The orientation error is not simply subtraction of your current pose and the desired pose. Given $\mathbf{R}_{a}, \ \mathbf{R}_{d} \in \text{SO}(3) $, where $\mathbf{R}_{d}$ is your desired orientation in matrix form and $\mathbf{R}_{a}$ is your current orientation in matrix form, you can compute the orientation error as:

$$ \mathbf{e}_{o} = 0.5 \cdot\sum_{i=1}^{3} \mathbf{r}_{a,i} \times \mathbf{r}_{d,i} $$

where $\mathbf{r}_{a,i}$ denotes the $i^{\text{th}}$ column vector of $\mathbf{R}_{a}$, likewise for $\mathbf{r}_{d,i}$ and $\mathbf{R}_{d}$, and $\times$ denotes the cross product. This will convert any rotation to the appropriate axis-angle representation of error - capable of being used in conjunction with the end-effector Jacobian.

---

The following image details converting from RPY velocity to angular velocity: