mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
cartesian demo: illustrate merging of trajectories
This commit is contained in:
parent
8debe68f99
commit
718ad320c2
@ -51,9 +51,12 @@ Task createTask() {
|
||||
t.stages()->setName("Cartesian Path");
|
||||
|
||||
const std::string group = "panda_arm";
|
||||
const std::string eef = "hand";
|
||||
|
||||
// create Cartesian interpolation "planner" to be used in stages
|
||||
auto cartesian = std::make_shared<solvers::CartesianPath>();
|
||||
// create Cartesian interpolation "planner" to be used in various stages
|
||||
auto cartesian_interpolation = std::make_shared<solvers::CartesianPath>();
|
||||
// create a joint-space interpolation "planner" to be used in various stages
|
||||
auto joint_interpolation = std::make_shared<solvers::JointInterpolationPlanner>();
|
||||
|
||||
// start from a fixed robot state
|
||||
t.loadRobotModel();
|
||||
@ -68,7 +71,7 @@ Task createTask() {
|
||||
}
|
||||
|
||||
{
|
||||
auto stage = std::make_unique<stages::MoveRelative>("x +0.2", cartesian);
|
||||
auto stage = std::make_unique<stages::MoveRelative>("x +0.2", cartesian_interpolation);
|
||||
stage->setGroup(group);
|
||||
geometry_msgs::Vector3Stamped direction;
|
||||
direction.header.frame_id = "world";
|
||||
@ -78,7 +81,7 @@ Task createTask() {
|
||||
}
|
||||
|
||||
{
|
||||
auto stage = std::make_unique<stages::MoveRelative>("y -0.3", cartesian);
|
||||
auto stage = std::make_unique<stages::MoveRelative>("y -0.3", cartesian_interpolation);
|
||||
stage->setGroup(group);
|
||||
geometry_msgs::Vector3Stamped direction;
|
||||
direction.header.frame_id = "world";
|
||||
@ -88,7 +91,7 @@ Task createTask() {
|
||||
}
|
||||
|
||||
{ // rotate about TCP
|
||||
auto stage = std::make_unique<stages::MoveRelative>("rz +45°", cartesian);
|
||||
auto stage = std::make_unique<stages::MoveRelative>("rz +45°", cartesian_interpolation);
|
||||
stage->setGroup(group);
|
||||
geometry_msgs::TwistStamped twist;
|
||||
twist.header.frame_id = "world";
|
||||
@ -98,16 +101,23 @@ Task createTask() {
|
||||
}
|
||||
|
||||
{ // perform a Cartesian motion, defined as a relative offset in joint space
|
||||
auto stage = std::make_unique<stages::MoveRelative>("joint offset", cartesian);
|
||||
auto stage = std::make_unique<stages::MoveRelative>("joint offset", cartesian_interpolation);
|
||||
stage->setGroup(group);
|
||||
std::map<std::string, double> offsets = { { "panda_joint1", M_PI / 6. }, { "panda_joint3", -M_PI / 6 } };
|
||||
stage->setDirection(offsets);
|
||||
t.add(std::move(stage));
|
||||
}
|
||||
|
||||
{ // move gripper into predefined open state
|
||||
auto stage = std::make_unique<stages::MoveTo>("open gripper", joint_interpolation);
|
||||
stage->setGroup(eef);
|
||||
stage->setGoal("open");
|
||||
t.add(std::move(stage));
|
||||
}
|
||||
|
||||
{ // move from reached state back to the original state, using joint interpolation
|
||||
auto joint_interpolation = std::make_shared<solvers::JointInterpolationPlanner>();
|
||||
stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation } };
|
||||
// specifying two groups (arm and hand) will try to merge both trajectories
|
||||
stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation }, { eef, joint_interpolation } };
|
||||
auto connect = std::make_unique<stages::Connect>("connect", planners);
|
||||
t.add(std::move(connect));
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user