Using MTC to Plan a Circular Path

[swiz23]

@JafarAbdi,

I'm having issues trying to use the ros2 branch of MTC to plan a circular path around a non-TCP frame (that already includes PR #322). I wrote up a question at ROS Answers describing the problem. Instead of copying and pasting it, I'm linking to it here. In a nutshell though the two main questions I have are:

• How to use MTC to plan a circular path around a non-TCP frame? Specifying the frame_id in a goal message to be the non-TCP frame has no impact (circular path is still generated only around the TCP frame).
• While rotating around the non-TCP frame, how to keep the orientation of the TCP constant?

[swiz23]

@JafarAbdi,

Here's a quick way to see the non-circular path currently generated for a non-TCP frame. I tested using the latest ros2 branch of MTC and main branch of moveit2 at the time of this writing. I did the following:

• Include the following code snippet in the panda.urdf file located here. This will add a 'non-TCP frame' called panda_ee_link as a child of the panda_hand link (currently, the SRDF defines the TCP frame as panda_link8). The 'panda_ee_link' frame is the frame, about which, I'd like to generate a purely rotational motion.

    <link name="panda_ee_link" />
    <joint name="panda_ee_joint" type="fixed">
        <origin rpy="0 0 0" xyz="0 0 0.3" />
        <parent link="panda_hand" />
        <child link="panda_ee_link" />
        <axis xyz="0 0 0" />
    </joint>

• Next, delete the specified lines in the modular.cpp file here and replace with the following code snippet. This shows the issue I'm seeing both with a MoveRelative stage and a MoveTo stage. However, I've seen the issue with a Connect / GeneratePose stage combo as well. Note that I'm using the setIKFrame function to set the target frame to panda_ee_link.

        {  // rotate about panda_ee_link using MoveRelative
                auto stage = std::make_unique<stages::MoveRelative>("rx +45°", cartesian);
                stage->properties().configureInitFrom(Stage::PARENT);
                geometry_msgs::msg::TwistStamped twist;
                twist.header.frame_id = "panda_ee_link";
                twist.twist.angular.x = M_PI / 4.;
                stage->setIKFrame("panda_ee_link");
                stage->setDirection(twist);
                c->insert(std::move(stage));
        }

        {  // rotate about panda_ee_link using MoveTo
                auto stage = std::make_unique<stages::MoveTo>("rx -90°", cartesian);
                stage->properties().configureInitFrom(Stage::PARENT);
                geometry_msgs::msg::PoseStamped pose;
                pose.header.frame_id = "panda_ee_link";
                pose.pose.orientation.x = -0.7071068;
                pose.pose.orientation.w = 0.7071068;
                stage->setIKFrame("panda_ee_link");
                stage->setGoal(pose);
                c->insert(std::move(stage));
        }

• After rebuilding, launch the demo by ros2 launch moveit_task_constructor_demo demo.launch.py. Then launch the 'modular' script by typing ros2 launch moveit_task_constructor_demo modular.launch.py.

The planning should be successful, but if you look at the 'panda_ee_link' frame during the 'rx +45°' and 'rx -90°' motion, you'll see that instead of the panda_ee_link frame rotating in place, there is also a translational component (which I wouldn't have expected).

[swiz23]

Thanks @rhaschke,

What would be the best way to test this? PR #380 is a ROS1 fix while PR #1547 is a ROS2 fix. I tried to go about it by using your moveit2 cartesian-interpolator branch, then merging the MTC fix-cartesian-interpolation branch into the ros2 branch of MTC, but ended up with merge conflicts.

[rhaschke]

If I find time tomorrow, I will try the merge.

[rhaschke]

@swiz23: See https://github.com/ros-planning/moveit_task_constructor/pull/382 for a ros2 fix. You will also need https://github.com/ros-planning/moveit2/pull/1547

[swiz23]

Thanks, I'll try this out!

[swiz23]

Finally got the time to try this out and it works as expected! I'd like to test this out on the code for my particular project. Will update here.

[swiz23]

Tested for my application successfully! I'll close this issue once the PR gets merged.

[sea-bass]

I'm actually working on using the Pilz motion planner as a planner within MTC that will give you a single circular arc trajectory, so you don't need a bunch of piecewise waypoints.

If this is of interest, I can keep this issue updated as I find time to flesh out my proof of concept.

[rhaschke]

Of course, the Pilz planner is another option to generate circular motions. But that wasn't the original issue here.

[swiz23]

@sea-bass, yep - that's definitely another option. But actually, in some cases (like the one here), you wouldn't need to specify a bunch of piece-wise way points. You would just specify a desired end-rotation around a desired frame, and that's it.

Looking forward to seeing #380 merged in soon :)

[swiz23]

Hey @rhaschke,

Thanks so much for merging that in! I assume that will be merged into the ros2 branch soon as well?

[rhaschke]

Yes, I‘m working on it…

[rhaschke]

@swiz23, I merged the master branch, including #380, into ros2.