I’m prototyping a geometric proportional plus integral controller that visually servos a tool mounted to the ur tool flange over a moving assembly. My control policy will interface to the UR through the speed_l command I found in section 13.1.39 of this documentation: ur-script-documentation. I’m wondering if the twist vector that is provided to this function is executed in the moving TCP frame or the base flange frame? To make my question more clear, suppose I send the hypthetical command:
speed_l([0,0,0,0,0,1.0], 1.0, 1.0)
Here the angular velocity requested is 1 radian per second about the z-axis. Will the result be that the robot rotates 1 radian per second about the base joint, or will wrist joint 3 rotate at 1 radian per second?
If the speed_l command is in the tcp frame, I would implement simple proportional control as follows:
# Error in sensor frame as element of SE(3)
error_vision_frame = ref_vision_pose * measured_vision_pose.inverse()
# Convert error to a vector in the lie algebra
error_vision_frame_as_twist = error_vision_frame.logarithm()
# Use proportional feedback control to convert error to corrective twist command
command_vision_frame = Kp * error_vision_frame_as_twist
#Convert command from vision frame to tcp frame. Adjoint here refers to the adjoint action of the sensor to tcp frame calibration pose on command in vision frame to convert it to tcp frame.
command_tcp_frame = sensor_to_tcp_calibration.adjoint(command_vision_frame)
#Is this correct?
ur_robot.speed_l(command_tcp_frame.to_ur_format(),0.1,0.008)
#Or should I push the command down to the base?
command_base_frame = tcp_to_base.adjoint(command_tcp_frame)
ur_robot.speed_l(command_base_frame.to_ur_format(),0.1,0.008)