Cartesian Pose/Velocity Control - Limit Violations
Closed this issue · 10 comments
Hi,
I am not sure if it is a franka_ros or a libfranka issue, but I'll start here.
libfranka 0.6.0, franka_ros 0.6.0
Filter on default (100,100,...)
cpu scaling disabled
communication test successful
I've implemented a velocity controller in ros following the cartesian velocity controller example. I calculated a trajectory to limit acceleration and jerk to the required constraints, but even then I got cartesian_motion_generator_acceleration_discontinuity errors. I lowered the constraints by factor 10 and got now mostly cartesian_motion_generator_joint_acceleration_discontinuity errors. By adding a simple low pass filter, I was able to get rid of these errors.
First question: How can I limit the jerk in joint space without knowing how the franka solves the inverse kinematics?
Then, I had some problems probably caused by noise, and wanted to switch to cartesian pose control. I started from the example and calculated my velocity the same way as before, but now added it to the pose like p = p + v * 0.001. (For simplification, translational motion only).
To get the pose, I first used initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d; in the starting() function like in the example.
Result: cartesian_motion_generator_joint_acceleration_discontinuity.
Second question: Why does that not work? Shouldn't acceleration and jerk be exactly the same as in the velocity controller?
-> I scaled down my velocities by factor 10 and decreased the cut-off frequency of my custom low pass filter
Result: errors less frequently, but existing. And very strange: after recovering from error, it kept moving for a while (without having input!), like the pose has internally still been updated during the first seconds of the error and it now after recovery wants to move on.
-> Okay, maybe it makes more sense to take the current robot state in the loop to prevent that "integrating" of the pose. So I changed my update() function to
pose_current = cartesian_pose_handle_->getRobotState().O_T_EE
p = pose_current + v * 0.001
Result: That consistently caused cartesian_motion_generator_joint_acceleration_discontinuity errors immediately.
->Okay, maybe try O_T_EE_d? Or increase the franka state publish rate?
Result: Same
->Then I realized (by printing out O_T_EE and O_T_EE_d) that there is a steady state error, they are not equal when my pose command is a constant pose.
Third question: How are O_T_EE and O_T_EE_d handled internally? Why there is a steady state error? What control strategy does the cartesian pose controller use?
Thanks in advance!
Today the pose control worked suddenly. Maybe it required a restart.. but for pose_current I had to use 0_T_EE_d. So, the second question is solved.
Hi, did you solve the issue of the cartesian_pose_example_controller facing discontinuity error? I'm facing the same problem mentioned: #84. Can you give me a little hint if possible? Thanks in advanced.
I am having similar issues with writing a custom cartesian pose controller for teleop. I've used a low pass filter but have had little luck. Any updates?
Well, I got my controller work yesyerday. Actually the guy above gave us the solution already. So the things I did are:
- Include the current robot pose using O_T_EE_d (this is important!) in the loop. And we will modulate this variable to give the new pose. For example, I want my robot to continuously in x direction. My code was
pose_current = cartesian_pose_handle_->getRobotState().O_T_EE;
delta_x += 0.0001;
pose_current[12] += delta_x * 0.001;(scale down to slow the movement)
Great news! Just to clarify, the cost you posted was what you had initially. Your new code instead has
pose_current = cartesian_pose_handle_->getRobotState().0_T_EE_d; is that correct?
Yes. And I included pose_current in the update function of the controller.
Looks like this works for me. Thank you!
Hi everyone @trannguyenle95 @i-abr @fwalch @jethrokuan @danneboom ,
Do you know what is the optimal way to stop the robot motion (pose X movement) after specific time/distance I tried many ways but I get error:
"terminate called after throwing an instance of 'franka::ControlException'
what(): libfranka: Move command aborted: motion aborted by reflex! ["cartesian_motion_generator_joint_velocity_discontinuity", "cartesian_motion_generator_joint_acceleration_discontinuity"]"
Best Regards,
Mahmoud
Have you tried to calculate a smooth stopping trajectory with limited acceleration and velocity? I guess you use cartesian control, so you can add an additional factor like 0.1 to reduce the acceleration in joint space even more. See the first part of my initial post.
Closing due to inactivity. Please open this git issue again if the topic still exists.