# cartesian velocity control loop implementation

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.

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:

• The vector resulting from your equation is the 3-element vector denoting the orientation error. If then I append it to the 3-element position error I finally get $\widetilde{x}$, right?
– dcfg
Sep 6, 2022 at 15:58
• by end-effector Jacobian you mean the geometric Jacobian?
– dcfg
Sep 6, 2022 at 16:36
• Yes to both of your questions! Sep 6, 2022 at 18:56
• Yes again! And conveniently enough it looks as though you have all the variables you need to do so. Sep 6, 2022 at 21:58
• Good question, the Jacobian is formulated wrt to the robot's base frame - so expressing the RPY derivatives in the base frame should do the trick! Sep 14, 2022 at 13:55