How to control the duration of single step motion in impedance control mode？

[wangzy0918]

Hi Dr. Matthias, thank you very much for your work. I have tried this controller on iiwa simulation and real robot. But now I have a question, how to control the movement duration of a single cloth in the torque control mode? It mainly has the following two purposes:

1. The speed can be controlled in a wide range of motion to ensure a balance between safety and efficiency.

2. Strike a balance between pose convergence accuracy and movement duration.
   At present, I simply wrote a class to publish the reference pose, as follows

   pose_pub = nh->advertise<geometry_msgs::PoseStamped>("/iiwa/CartesianImpedance_trajectory_controller/reference_pose",10)

   void iiwa_pose_param_update::set_iiwa_pose_msg() {
   ref_rotation.GetQuaternion(quaternion_x,quaternion_y,quaternion_z,quaternion_w);
   pose_msg.pose.position.x = ref_pose.x();
   pose_msg.pose.position.y = ref_pose.y();
   pose_msg.pose.position.z = ref_pose.z();
   pose_msg.pose.orientation.x = quaternion_x;
   pose_msg.pose.orientation.y = quaternion_y;
   pose_msg.pose.orientation.z = quaternion_z;
   pose_msg.pose.orientation.w = quaternion_w;
   pose_msg.header.stamp = ros::Time::now();
   }

   void * iiwa_pose_param_update::publish_pose_config(const KDL::Frame& ref_frame) {
   ref_pose = ref_frame.p;
   ref_rotation = ref_frame.M;
   this->set_iiwa_pose_msg();
   pose_pub.publish(pose_msg);
   ros::spinOnce();
   ROS_INFO("Published pos message.");
   return nullptr;}

However, in the main program, I can only continue to execute the release topic before it can better converge to the desired pose, and the convergence time is not fixed (affected by the movement distance), as follows

 while (ros::ok()){
     ipu->publish_pose_config(KDL::Frame(KDL::Rotation::RPY(0, M_PI, 0),
                                         KDL::Vector(0.5, 0.0, 0.7)));

 }

Questions are as follows:

1. I don’t know if this has anything to do with the choice of communication mechanism. Maybe the action communication mechanism can be used to control the single step duration? I'm not sure about this yet, and I still need to understand the action communication mechanism.
2. Is it possible to manually set the pose convergence threshold and maximum movement duration? In this way, even if the movement time cannot be accurately controlled, the problem of movement accuracy can still be solved.

I would be very grateful if you could provide some suggestions or solutions. In addition, I hope you can correct me if I don't understand your code properly.

[matthias-mayr]

Great to hear that this project is of use for you.

Tracking the position, deviations and "arrival" is a bit tricky with compliant control since it is often context specific. I deliberately left it outside this controller implementation since it depends on the use case and user preferences.

One relatively easy way to achieve what you're looking for is another repository that also lives here:
https://github.com/matthias-mayr/cartesian_trajectory_generator
It works well with this controller. It does not implement a maximum duration, but it would be easy to add. With this repo, you can also send an action and get a reply once the arm is sufficiently close, which is checked with some preset thresholds.

Feel free to post if this does not meet your needs. If it does, feel free to close the issue.

[wangzy0918]

Thank you very much for your reply. I think I understand what you mean. I will try the trajectory generator you mentioned, but I have other work to do now, so I estimate it will take until next month. I will close this issue now, and if there are any issues, I will consult with you again. Thank you again for your reply. Wishing you a nice day！