Using MTC to Plan a Circular Path

edit

Hi,

I'm currently trying to use the ros2 branch of MTC (MoveIt Task Constructor) to plan a circular motion to open a door. I know that it's possible to use the MoveTo or MoveRelative stages to make an arm rotate around the TCP frame by just specifying the desired goal orientation in a PoseStamped message (MoveTo) or angular rotation in a TwistStamped message (MoveRelative) - as shown in the Cartesian demo code.

So what I'm thinking of doing is adding a fixed frame in the robot URDF (let's call it the door_hinge_frame) that is a child of one of the robot's links (not necessarily the TCP frame though). The door_hinge_frame would be located at the physical joint of the door when the TCP is in the desired 'opening' pose. Then, by just specifying the MoveTo or MoveRelative message frame_id to the door_hinge_frame, and specifying the goal pose to 90 degrees, a circular motion should be planned that opens the door.

So a few questions:

• Is this the correct way to perform this motion or is there an easier way? If so, what would that way be?
• While trying to test this approach out, the resulting motion via MTC is not the one that I expected. Instead of performing a circular motion around the door_hinge_frame, the arm still tries to rotate around the TCP frame. I've also tried to set the stage IK frame (via stage->setIKFrame()) to the door_hinge_frame, but that results in non-circular trajectories around the hinge joint.

• In the above example, the TCP frame should move as shown in the left-hand part of the image below. But let's say I want the desired motion to be more like the right-hand part. How would I achieve that?

  If I was using the C++ MoveGroupInterace API, I could achieve this by sending a static transform to the TF tree of where the door hinge frame is located relative to a world frame (in which case, I'd just delete the door_hinge_frame in the robot URDF). Then I could create 90 or so waypoints that define the pose of the TCP relative to the door hinge frame such that the orientation of the TCP is as shown in the right-hand part of the image. Next, I could call the computeCartesianPath function with the waypoints and generate the trajectory. However, the CartesianPath class in MTC does not include a 'plan' function that allows passing in waypoints. What I could do instead is just create a Serial container of MoveTo stages in which I define the 90 or so waypoints. However, then the arm would accelerate/decelerate to each of the 90 waypoints instead of creating one trajectory where the arm would would only accelerate/decelerate once...

Any help with answering these questions would be much appreciated! Note that I'm working in a Docker container running Ubuntu 20.04 and ROS Galactic (I've also seen this issue in a Docker container running Ubuntu 22.04 and ... (more)