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 theangular velocity
position
data coming from my mouse. The z coordinate is controlled through the mouse wheellinear 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.