cartesian demo: illustrate merging of trajectories

This commit is contained in:
Robert Haschke 2020-03-04 10:25:04 +01:00
parent 8debe68f99
commit 718ad320c2

View File

@ -51,9 +51,12 @@ Task createTask() {
t.stages()->setName("Cartesian Path"); t.stages()->setName("Cartesian Path");
const std::string group = "panda_arm"; const std::string group = "panda_arm";
const std::string eef = "hand";
// create Cartesian interpolation "planner" to be used in stages // create Cartesian interpolation "planner" to be used in various stages
auto cartesian = std::make_shared<solvers::CartesianPath>(); 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 // start from a fixed robot state
t.loadRobotModel(); 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); stage->setGroup(group);
geometry_msgs::Vector3Stamped direction; geometry_msgs::Vector3Stamped direction;
direction.header.frame_id = "world"; 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); stage->setGroup(group);
geometry_msgs::Vector3Stamped direction; geometry_msgs::Vector3Stamped direction;
direction.header.frame_id = "world"; direction.header.frame_id = "world";
@ -88,7 +91,7 @@ Task createTask() {
} }
{ // rotate about TCP { // 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); stage->setGroup(group);
geometry_msgs::TwistStamped twist; geometry_msgs::TwistStamped twist;
twist.header.frame_id = "world"; twist.header.frame_id = "world";
@ -98,16 +101,23 @@ Task createTask() {
} }
{ // perform a Cartesian motion, defined as a relative offset in joint space { // 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); stage->setGroup(group);
std::map<std::string, double> offsets = { { "panda_joint1", M_PI / 6. }, { "panda_joint3", -M_PI / 6 } }; std::map<std::string, double> offsets = { { "panda_joint1", M_PI / 6. }, { "panda_joint3", -M_PI / 6 } };
stage->setDirection(offsets); stage->setDirection(offsets);
t.add(std::move(stage)); 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 { // move from reached state back to the original state, using joint interpolation
auto joint_interpolation = std::make_shared<solvers::JointInterpolationPlanner>(); // specifying two groups (arm and hand) will try to merge both trajectories
stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation } }; stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation }, { eef, joint_interpolation } };
auto connect = std::make_unique<stages::Connect>("connect", planners); auto connect = std::make_unique<stages::Connect>("connect", planners);
t.add(std::move(connect)); t.add(std::move(connect));
} }