mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
removed old stages: Move, Gripper, CartesianPositionMotion
This commit is contained in:
parent
b22217deab
commit
a044b719ac
@ -1,14 +1,12 @@
|
|||||||
#include <moveit/task_constructor/task.h>
|
#include <moveit/task_constructor/task.h>
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/current_state.h>
|
#include <moveit/task_constructor/stages/current_state.h>
|
||||||
#include <moveit/task_constructor/stages/gripper.h>
|
#include <moveit/task_constructor/stages/simple_grasp.h>
|
||||||
#include <moveit/task_constructor/stages/move.h>
|
#include <moveit/task_constructor/stages/pick.h>
|
||||||
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
#include <moveit/task_constructor/stages/connect.h>
|
||||||
#include <moveit/task_constructor/stages/cartesian_position_motion.h>
|
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
||||||
#include <moveit/task_constructor/stages/compute_ik.h>
|
|
||||||
|
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
|
|
||||||
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
@ -40,82 +38,45 @@ int main(int argc, char** argv){
|
|||||||
spawnObject();
|
spawnObject();
|
||||||
|
|
||||||
Task t;
|
Task t;
|
||||||
// define global properties used by most stages
|
|
||||||
t.setProperty("group", std::string("arm"));
|
|
||||||
t.setProperty("eef", std::string("gripper"));
|
|
||||||
t.setProperty("planner", std::string("RRTConnectkConfigDefault"));
|
|
||||||
t.setProperty("link", std::string("s_model_tool0"));
|
|
||||||
|
|
||||||
Stage* initial_stage = nullptr;
|
Stage* initial_stage = nullptr;
|
||||||
|
|
||||||
{
|
|
||||||
auto initial = std::make_unique<stages::CurrentState>("current state");
|
auto initial = std::make_unique<stages::CurrentState>("current state");
|
||||||
initial_stage = initial.get();
|
initial_stage = initial.get();
|
||||||
t.add(std::move(initial));
|
t.add(std::move(initial));
|
||||||
}
|
|
||||||
|
|
||||||
{
|
// planner used for connect
|
||||||
auto move = std::make_unique<stages::Gripper>("open gripper");
|
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
|
||||||
move->properties().configureInitFrom(Stage::PARENT);
|
pipeline->setTimeout(8.0);
|
||||||
move->setTo("open");
|
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||||
t.add(std::move(move));
|
// connect to pick
|
||||||
}
|
stages::Connect::GroupPlannerVector planners = {{"arm", pipeline}, {"gripper", pipeline}};
|
||||||
|
auto connect = std::make_unique<stages::Connect>("connect", planners);
|
||||||
|
connect->properties().configureInitFrom(Stage::PARENT);
|
||||||
|
t.add(std::move(connect));
|
||||||
|
|
||||||
{
|
// grasp generator
|
||||||
auto move = std::make_unique<stages::Move>("move to pre-grasp");
|
auto grasp_generator = std::make_unique<stages::SimpleGrasp>();
|
||||||
move->properties().configureInitFrom(Stage::PARENT);
|
grasp_generator->setIKFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
||||||
move->setTimeout(8.0);
|
grasp_generator->setMaxIKSolutions(8);
|
||||||
t.add(std::move(move));
|
grasp_generator->setAngleDelta(.2);
|
||||||
}
|
grasp_generator->setPreGraspPose("open");
|
||||||
|
grasp_generator->setGraspPose("closed");
|
||||||
|
grasp_generator->setMonitoredStage(initial_stage);
|
||||||
|
|
||||||
{
|
auto pick = std::make_unique<stages::Pick>(std::move(grasp_generator));
|
||||||
auto move = std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
|
pick->setProperty("eef", std::string("gripper"));
|
||||||
// move->addSolutionCallback(std::bind(&Introspection::publishSolution, &t.introspection(), std::placeholders::_1));
|
pick->setProperty("object", std::string("object"));
|
||||||
move->properties().configureInitFrom(Stage::PARENT);
|
geometry_msgs::TwistStamped approach;
|
||||||
move->setMinMaxDistance(.03, 0.1);
|
approach.header.frame_id = "s_model_tool0";
|
||||||
move->setCartesianStepSize(0.02);
|
approach.twist.linear.x = 1.0;
|
||||||
|
pick->setApproachMotion(approach, 0.03, 0.1);
|
||||||
|
|
||||||
geometry_msgs::PointStamped target;
|
geometry_msgs::TwistStamped lift;
|
||||||
target.header.frame_id= "object";
|
lift.header.frame_id = "world";
|
||||||
move->towards(target);
|
lift.twist.linear.z = 1.0;
|
||||||
t.add(std::move(move));
|
pick->setLiftMotion(lift, 0.03, 0.05);
|
||||||
}
|
|
||||||
|
|
||||||
{
|
t.add(std::move(pick));
|
||||||
auto gengrasp = std::make_unique<stages::GenerateGraspPose>("generate grasp pose");
|
|
||||||
gengrasp->properties().configureInitFrom(Stage::PARENT);
|
|
||||||
gengrasp->setNamedPose("open");
|
|
||||||
gengrasp->setObject("object");
|
|
||||||
gengrasp->setAngleDelta(-.2);
|
|
||||||
gengrasp->setMonitoredStage(initial_stage);
|
|
||||||
|
|
||||||
auto ik = std::make_unique<stages::ComputeIK>("compute ik", std::move(gengrasp));
|
|
||||||
ik->properties().configureInitFrom(Stage::PARENT, {"eef"});
|
|
||||||
ik->setMaxIKSolutions(8);
|
|
||||||
ik->setIKFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
|
||||||
t.add(std::move(ik));
|
|
||||||
}
|
|
||||||
|
|
||||||
{
|
|
||||||
auto move = std::make_unique<stages::Gripper>("grasp");
|
|
||||||
move->properties().configureInitFrom(Stage::PARENT);
|
|
||||||
move->setTo("closed");
|
|
||||||
move->graspObject("object");
|
|
||||||
t.add(std::move(move));
|
|
||||||
}
|
|
||||||
|
|
||||||
{
|
|
||||||
auto move = std::make_unique<stages::CartesianPositionMotion>("lift object");
|
|
||||||
move->properties().configureInitFrom(Stage::PARENT);
|
|
||||||
move->setMinMaxDistance(0.03, 0.05);
|
|
||||||
move->setCartesianStepSize(0.01);
|
|
||||||
|
|
||||||
geometry_msgs::Vector3Stamped direction;
|
|
||||||
direction.header.frame_id= "world";
|
|
||||||
direction.vector.z= 1.0;
|
|
||||||
move->along(direction);
|
|
||||||
t.add(std::move(move));
|
|
||||||
}
|
|
||||||
|
|
||||||
try {
|
try {
|
||||||
t.plan();
|
t.plan();
|
||||||
|
|||||||
@ -1,79 +0,0 @@
|
|||||||
/*********************************************************************
|
|
||||||
* Software License Agreement (BSD License)
|
|
||||||
*
|
|
||||||
* Copyright (c) 2017, Hamburg University
|
|
||||||
* All rights reserved.
|
|
||||||
*
|
|
||||||
* Redistribution and use in source and binary forms, with or without
|
|
||||||
* modification, are permitted provided that the following conditions
|
|
||||||
* are met:
|
|
||||||
*
|
|
||||||
* * Redistributions of source code must retain the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer.
|
|
||||||
* * Redistributions in binary form must reproduce the above
|
|
||||||
* copyright notice, this list of conditions and the following
|
|
||||||
* disclaimer in the documentation and/or other materials provided
|
|
||||||
* with the distribution.
|
|
||||||
* * Neither the name of Bielefeld University nor the names of its
|
|
||||||
* contributors may be used to endorse or promote products derived
|
|
||||||
* from this software without specific prior written permission.
|
|
||||||
*
|
|
||||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
||||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
||||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
*********************************************************************/
|
|
||||||
|
|
||||||
/* Authors: Michael Goerner
|
|
||||||
Desc: Stage to plan linear cartesian motions
|
|
||||||
*/
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <moveit/task_constructor/stage.h>
|
|
||||||
|
|
||||||
#include <geometry_msgs/Vector3Stamped.h>
|
|
||||||
#include <geometry_msgs/PointStamped.h>
|
|
||||||
|
|
||||||
namespace moveit {
|
|
||||||
namespace planning_interface { MOVEIT_CLASS_FORWARD(MoveGroupInterface) }
|
|
||||||
namespace core { MOVEIT_CLASS_FORWARD(RobotState) }
|
|
||||||
}
|
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
|
||||||
|
|
||||||
class CartesianPositionMotion : public PropagatingEitherWay {
|
|
||||||
public:
|
|
||||||
CartesianPositionMotion(std::string name);
|
|
||||||
|
|
||||||
virtual bool computeForward(const InterfaceState &from) override;
|
|
||||||
virtual bool computeBackward(const InterfaceState &to) override;
|
|
||||||
|
|
||||||
void setGroup(std::string group);
|
|
||||||
void setLink(std::string link);
|
|
||||||
|
|
||||||
void setMinDistance(double distance);
|
|
||||||
void setMaxDistance(double distance);
|
|
||||||
void setMinMaxDistance(double min_distance, double max_distance);
|
|
||||||
|
|
||||||
void towards(geometry_msgs::PointStamped goal);
|
|
||||||
void along(geometry_msgs::Vector3Stamped direction);
|
|
||||||
|
|
||||||
void setCartesianStepSize(double distance);
|
|
||||||
|
|
||||||
protected:
|
|
||||||
enum {
|
|
||||||
MODE_ALONG= 1,
|
|
||||||
MODE_TOWARDS= 2
|
|
||||||
} mode_;
|
|
||||||
};
|
|
||||||
|
|
||||||
} } }
|
|
||||||
@ -1,74 +0,0 @@
|
|||||||
/*********************************************************************
|
|
||||||
* Software License Agreement (BSD License)
|
|
||||||
*
|
|
||||||
* Copyright (c) 2017, Hamburg University
|
|
||||||
* All rights reserved.
|
|
||||||
*
|
|
||||||
* Redistribution and use in source and binary forms, with or without
|
|
||||||
* modification, are permitted provided that the following conditions
|
|
||||||
* are met:
|
|
||||||
*
|
|
||||||
* * Redistributions of source code must retain the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer.
|
|
||||||
* * Redistributions in binary form must reproduce the above
|
|
||||||
* copyright notice, this list of conditions and the following
|
|
||||||
* disclaimer in the documentation and/or other materials provided
|
|
||||||
* with the distribution.
|
|
||||||
* * Neither the name of Bielefeld University nor the names of its
|
|
||||||
* contributors may be used to endorse or promote products derived
|
|
||||||
* from this software without specific prior written permission.
|
|
||||||
*
|
|
||||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
||||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
||||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
*********************************************************************/
|
|
||||||
|
|
||||||
/* Authors: Michael Goerner
|
|
||||||
Desc: Simple stage to grasp objects by closing the gripper
|
|
||||||
*/
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <moveit/task_constructor/stage.h>
|
|
||||||
|
|
||||||
namespace moveit { namespace planning_interface {
|
|
||||||
MOVEIT_CLASS_FORWARD(MoveGroupInterface)
|
|
||||||
} }
|
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
|
||||||
|
|
||||||
class Gripper : public PropagatingEitherWay {
|
|
||||||
public:
|
|
||||||
Gripper(std::string name);
|
|
||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model);
|
|
||||||
bool computeForward(const InterfaceState& from) override;
|
|
||||||
bool computeBackward(const InterfaceState& to) override;
|
|
||||||
|
|
||||||
void setEndEffector(std::string eef);
|
|
||||||
void setAttachLink(std::string link);
|
|
||||||
|
|
||||||
void setFrom(std::string named_target);
|
|
||||||
void setTo(std::string named_target);
|
|
||||||
|
|
||||||
void graspObject(std::string grasp_object);
|
|
||||||
|
|
||||||
protected:
|
|
||||||
bool compute(const InterfaceState &state, planning_scene::PlanningScenePtr &scene,
|
|
||||||
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost, Direction dir);
|
|
||||||
|
|
||||||
protected:
|
|
||||||
planning_pipeline::PlanningPipelinePtr planner_;
|
|
||||||
moveit::planning_interface::MoveGroupInterfacePtr mgi_;
|
|
||||||
};
|
|
||||||
|
|
||||||
} } }
|
|
||||||
@ -1,60 +0,0 @@
|
|||||||
/*********************************************************************
|
|
||||||
* Software License Agreement (BSD License)
|
|
||||||
*
|
|
||||||
* Copyright (c) 2017, Hamburg University
|
|
||||||
* All rights reserved.
|
|
||||||
*
|
|
||||||
* Redistribution and use in source and binary forms, with or without
|
|
||||||
* modification, are permitted provided that the following conditions
|
|
||||||
* are met:
|
|
||||||
*
|
|
||||||
* * Redistributions of source code must retain the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer.
|
|
||||||
* * Redistributions in binary form must reproduce the above
|
|
||||||
* copyright notice, this list of conditions and the following
|
|
||||||
* disclaimer in the documentation and/or other materials provided
|
|
||||||
* with the distribution.
|
|
||||||
* * Neither the name of Bielefeld University nor the names of its
|
|
||||||
* contributors may be used to endorse or promote products derived
|
|
||||||
* from this software without specific prior written permission.
|
|
||||||
*
|
|
||||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
||||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
||||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
*********************************************************************/
|
|
||||||
|
|
||||||
/* Authors: Michael Goerner
|
|
||||||
Desc: Generic Stage to connect arbitrary states by motion planning
|
|
||||||
*/
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#include <moveit/task_constructor/stage.h>
|
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
|
||||||
|
|
||||||
class Move : public Connecting {
|
|
||||||
public:
|
|
||||||
Move(std::string name);
|
|
||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model);
|
|
||||||
bool compute(const InterfaceState &from, const InterfaceState &to);
|
|
||||||
|
|
||||||
void setGroup(const std::string &group);
|
|
||||||
void setPlannerId(const std::string &planner);
|
|
||||||
void setTimeout(double timeout);
|
|
||||||
|
|
||||||
protected:
|
|
||||||
planning_pipeline::PlanningPipelinePtr planner_;
|
|
||||||
};
|
|
||||||
|
|
||||||
} } }
|
|
||||||
@ -15,10 +15,6 @@ add_library(${PROJECT_NAME}_stages
|
|||||||
${PROJECT_INCLUDE}/stages/simple_grasp.h
|
${PROJECT_INCLUDE}/stages/simple_grasp.h
|
||||||
${PROJECT_INCLUDE}/stages/pick.h
|
${PROJECT_INCLUDE}/stages/pick.h
|
||||||
|
|
||||||
${PROJECT_INCLUDE}/stages/cartesian_position_motion.h
|
|
||||||
${PROJECT_INCLUDE}/stages/gripper.h
|
|
||||||
${PROJECT_INCLUDE}/stages/move.h
|
|
||||||
|
|
||||||
modify_planning_scene.cpp
|
modify_planning_scene.cpp
|
||||||
fix_collision_objects.cpp
|
fix_collision_objects.cpp
|
||||||
|
|
||||||
@ -34,10 +30,6 @@ add_library(${PROJECT_NAME}_stages
|
|||||||
|
|
||||||
simple_grasp.cpp
|
simple_grasp.cpp
|
||||||
pick.cpp
|
pick.cpp
|
||||||
|
|
||||||
cartesian_position_motion.cpp
|
|
||||||
gripper.cpp
|
|
||||||
move.cpp
|
|
||||||
)
|
)
|
||||||
target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES})
|
target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||||
|
|
||||||
|
|||||||
@ -1,265 +0,0 @@
|
|||||||
/*********************************************************************
|
|
||||||
* Software License Agreement (BSD License)
|
|
||||||
*
|
|
||||||
* Copyright (c) 2017, Hamburg University
|
|
||||||
* All rights reserved.
|
|
||||||
*
|
|
||||||
* Redistribution and use in source and binary forms, with or without
|
|
||||||
* modification, are permitted provided that the following conditions
|
|
||||||
* are met:
|
|
||||||
*
|
|
||||||
* * Redistributions of source code must retain the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer.
|
|
||||||
* * Redistributions in binary form must reproduce the above
|
|
||||||
* copyright notice, this list of conditions and the following
|
|
||||||
* disclaimer in the documentation and/or other materials provided
|
|
||||||
* with the distribution.
|
|
||||||
* * Neither the name of Bielefeld University nor the names of its
|
|
||||||
* contributors may be used to endorse or promote products derived
|
|
||||||
* from this software without specific prior written permission.
|
|
||||||
*
|
|
||||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
||||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
||||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
*********************************************************************/
|
|
||||||
|
|
||||||
/* Authors: Michael Goerner */
|
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/cartesian_position_motion.h>
|
|
||||||
#include <moveit/task_constructor/storage.h>
|
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
|
||||||
#include <moveit/robot_state/conversions.h>
|
|
||||||
#include <moveit/robot_state/robot_state.h>
|
|
||||||
#include <moveit/move_group_interface/move_group_interface.h>
|
|
||||||
|
|
||||||
#include <eigen_conversions/eigen_msg.h>
|
|
||||||
|
|
||||||
#include <Eigen/Geometry>
|
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
|
||||||
|
|
||||||
CartesianPositionMotion::CartesianPositionMotion(std::string name)
|
|
||||||
: PropagatingEitherWay(name)
|
|
||||||
{
|
|
||||||
auto& p = properties();
|
|
||||||
p.declare<std::string>("group", "name of planning group");
|
|
||||||
p.declare<std::string>("link", "name of link used for IK");
|
|
||||||
p.declare<double>("min_distance", "minimum distance to move");
|
|
||||||
p.declare<double>("max_distance", "maximum distance to move");
|
|
||||||
p.declare<double>("step_size", 0.005);
|
|
||||||
p.declare<geometry_msgs::PointStamped>("towards", "target point of motion");
|
|
||||||
p.declare<geometry_msgs::Vector3Stamped>("along", "vector along which to move");
|
|
||||||
}
|
|
||||||
|
|
||||||
void CartesianPositionMotion::setGroup(std::string group){
|
|
||||||
setProperty("group", group);
|
|
||||||
}
|
|
||||||
|
|
||||||
void CartesianPositionMotion::setLink(std::string link){
|
|
||||||
setProperty("link", link);
|
|
||||||
}
|
|
||||||
|
|
||||||
void CartesianPositionMotion::setMinDistance(double distance){
|
|
||||||
setProperty("min_distance", distance);
|
|
||||||
}
|
|
||||||
|
|
||||||
void CartesianPositionMotion::setMaxDistance(double distance){
|
|
||||||
setProperty("max_distance", distance);
|
|
||||||
}
|
|
||||||
|
|
||||||
void CartesianPositionMotion::setMinMaxDistance(double min_distance, double max_distance){
|
|
||||||
setProperty("min_distance", min_distance);
|
|
||||||
setProperty("max_distance", max_distance);
|
|
||||||
}
|
|
||||||
|
|
||||||
void CartesianPositionMotion::towards(geometry_msgs::PointStamped towards){
|
|
||||||
mode_= CartesianPositionMotion::MODE_TOWARDS;
|
|
||||||
setProperty("towards", towards);
|
|
||||||
}
|
|
||||||
|
|
||||||
void CartesianPositionMotion::along(geometry_msgs::Vector3Stamped along){
|
|
||||||
mode_= CartesianPositionMotion::MODE_ALONG;
|
|
||||||
setProperty("along", along);
|
|
||||||
}
|
|
||||||
|
|
||||||
void CartesianPositionMotion::setCartesianStepSize(double distance){
|
|
||||||
setProperty("step_size", distance);
|
|
||||||
}
|
|
||||||
|
|
||||||
namespace {
|
|
||||||
bool isValid(planning_scene::PlanningSceneConstPtr scene,
|
|
||||||
robot_state::RobotState* state,
|
|
||||||
const robot_model::JointModelGroup* jmg,
|
|
||||||
const double* joint_positions){
|
|
||||||
state->setJointGroupPositions(jmg, joint_positions);
|
|
||||||
state->update();
|
|
||||||
return !scene->isStateColliding(const_cast<const robot_state::RobotState&>(*state), jmg->getName());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
|
||||||
planning_scene::PlanningScenePtr result_scene = from.scene()->diff();
|
|
||||||
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
|
||||||
|
|
||||||
const auto& props = properties();
|
|
||||||
const std::string& group = props.get<std::string>("group");
|
|
||||||
const std::string& link = props.get<std::string>("link");
|
|
||||||
|
|
||||||
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group);
|
|
||||||
const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link);
|
|
||||||
|
|
||||||
const moveit::core::GroupStateValidityCallbackFn is_valid=
|
|
||||||
std::bind(
|
|
||||||
&isValid,
|
|
||||||
result_scene,
|
|
||||||
std::placeholders::_1,
|
|
||||||
std::placeholders::_2,
|
|
||||||
std::placeholders::_3);
|
|
||||||
|
|
||||||
std::vector<moveit::core::RobotStatePtr> trajectory_steps;
|
|
||||||
bool succeeded= false;
|
|
||||||
|
|
||||||
if( mode_ == CartesianPositionMotion::MODE_TOWARDS ){
|
|
||||||
const geometry_msgs::PointStamped& towards = props.get<geometry_msgs::PointStamped>("towards");
|
|
||||||
const Eigen::Affine3d& frame= from.scene()->getFrameTransform(towards.header.frame_id);
|
|
||||||
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link);
|
|
||||||
|
|
||||||
Eigen::Vector3d target_point;
|
|
||||||
tf::pointMsgToEigen(towards.point, target_point);
|
|
||||||
target_point= frame*target_point;
|
|
||||||
|
|
||||||
// retain orientation of link
|
|
||||||
Eigen::Affine3d target= link_pose;
|
|
||||||
target.translation()= target_point;
|
|
||||||
|
|
||||||
double achieved_fraction= robot_state.computeCartesianPath(
|
|
||||||
jmg,
|
|
||||||
trajectory_steps,
|
|
||||||
link_model,
|
|
||||||
target,
|
|
||||||
true, /* global frame */
|
|
||||||
props.get<double>("step_size"), /* cartesian step size */
|
|
||||||
1.5, /* jump threshold */
|
|
||||||
is_valid);
|
|
||||||
|
|
||||||
const double achieved_distance= achieved_fraction*(link_pose.translation()-target_point).norm();
|
|
||||||
|
|
||||||
ROS_INFO_NAMED(this->name().c_str(), "achieved %f of cartesian motion", achieved_distance);
|
|
||||||
|
|
||||||
succeeded= achieved_distance >= props.get<double>("min_distance");
|
|
||||||
}
|
|
||||||
else if( mode_ == CartesianPositionMotion::MODE_ALONG ){
|
|
||||||
const geometry_msgs::Vector3Stamped& along = props.get<geometry_msgs::Vector3Stamped>("along");
|
|
||||||
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along.header.frame_id);
|
|
||||||
Eigen::Vector3d direction;
|
|
||||||
tf::vectorMsgToEigen(along.vector, direction);
|
|
||||||
direction= frame.linear()*direction;
|
|
||||||
|
|
||||||
double achieved_distance= robot_state.computeCartesianPath(
|
|
||||||
jmg,
|
|
||||||
trajectory_steps,
|
|
||||||
link_model,
|
|
||||||
direction,
|
|
||||||
true, /* global frame */
|
|
||||||
props.get<double>("max_distance"),
|
|
||||||
props.get<double>("step_size"), /* cartesian step size */
|
|
||||||
1.5, /* jump threshold */
|
|
||||||
is_valid);
|
|
||||||
|
|
||||||
ROS_INFO_NAMED(this->name().c_str(), "achieved %f of cartesian motion", achieved_distance);
|
|
||||||
|
|
||||||
succeeded= achieved_distance >= props.get<double>("min_distance");
|
|
||||||
}
|
|
||||||
else
|
|
||||||
throw std::runtime_error("position motion has neither a goal nor a direction");
|
|
||||||
|
|
||||||
|
|
||||||
if(succeeded){
|
|
||||||
robot_trajectory::RobotTrajectoryPtr traj
|
|
||||||
= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
|
||||||
for( auto& tp : trajectory_steps )
|
|
||||||
traj->addSuffixWayPoint(tp, 0.0);
|
|
||||||
sendForward(from, InterfaceState(result_scene), SubTrajectory(traj));
|
|
||||||
}
|
|
||||||
|
|
||||||
return succeeded;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
|
|
||||||
planning_scene::PlanningScenePtr result_scene = to.scene()->diff();
|
|
||||||
robot_state::RobotState &robot_state = result_scene->getCurrentStateNonConst();
|
|
||||||
|
|
||||||
const auto& props = properties();
|
|
||||||
const std::string& group = props.get<std::string>("group");
|
|
||||||
const std::string& link = props.get<std::string>("link");
|
|
||||||
|
|
||||||
const moveit::core::JointModelGroup* jmg= robot_state.getJointModelGroup(group);
|
|
||||||
const moveit::core::LinkModel* link_model= robot_state.getRobotModel()->getLinkModel(link);
|
|
||||||
|
|
||||||
const moveit::core::GroupStateValidityCallbackFn is_valid=
|
|
||||||
std::bind(
|
|
||||||
&isValid,
|
|
||||||
result_scene,
|
|
||||||
std::placeholders::_1,
|
|
||||||
std::placeholders::_2,
|
|
||||||
std::placeholders::_3);
|
|
||||||
|
|
||||||
Eigen::Vector3d direction;
|
|
||||||
|
|
||||||
switch(mode_){
|
|
||||||
case(CartesianPositionMotion::MODE_TOWARDS):
|
|
||||||
{
|
|
||||||
const Eigen::Affine3d& link_pose= robot_state.getGlobalLinkTransform(link);
|
|
||||||
direction= link_pose.linear()*Eigen::Vector3d(-1,0,0);
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case(CartesianPositionMotion::MODE_ALONG):
|
|
||||||
{
|
|
||||||
const geometry_msgs::Vector3Stamped& along = props.get<geometry_msgs::Vector3Stamped>("along");
|
|
||||||
const Eigen::Affine3d& frame= robot_state.getGlobalLinkTransform(along.header.frame_id);
|
|
||||||
tf::vectorMsgToEigen(along.vector, direction);
|
|
||||||
direction= frame.linear()*direction;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
throw std::runtime_error("position motion has neither a goal nor a direction");
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<moveit::core::RobotStatePtr> trajectory_steps;
|
|
||||||
|
|
||||||
double achieved_distance= robot_state.computeCartesianPath(
|
|
||||||
jmg,
|
|
||||||
trajectory_steps,
|
|
||||||
link_model,
|
|
||||||
direction,
|
|
||||||
true, /* global frame */
|
|
||||||
props.get<double>("max_distance"),
|
|
||||||
props.get<double>("step_size"), /* cartesian step size */
|
|
||||||
1.5, /* jump threshold */
|
|
||||||
is_valid);
|
|
||||||
|
|
||||||
ROS_INFO_NAMED(this->name().c_str(), "achieved %f of cartesian motion", achieved_distance);
|
|
||||||
|
|
||||||
bool succeeded= achieved_distance >= props.get<double>("min_distance");
|
|
||||||
|
|
||||||
if(succeeded){
|
|
||||||
robot_trajectory::RobotTrajectoryPtr traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
|
||||||
for( auto& tp : trajectory_steps )
|
|
||||||
traj->addPrefixWayPoint(tp, 0.0);
|
|
||||||
sendBackward(InterfaceState(result_scene), to, SubTrajectory(traj));
|
|
||||||
}
|
|
||||||
|
|
||||||
return succeeded;
|
|
||||||
}
|
|
||||||
|
|
||||||
} } }
|
|
||||||
@ -1,164 +0,0 @@
|
|||||||
/*********************************************************************
|
|
||||||
* Software License Agreement (BSD License)
|
|
||||||
*
|
|
||||||
* Copyright (c) 2017, Hamburg University
|
|
||||||
* All rights reserved.
|
|
||||||
*
|
|
||||||
* Redistribution and use in source and binary forms, with or without
|
|
||||||
* modification, are permitted provided that the following conditions
|
|
||||||
* are met:
|
|
||||||
*
|
|
||||||
* * Redistributions of source code must retain the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer.
|
|
||||||
* * Redistributions in binary form must reproduce the above
|
|
||||||
* copyright notice, this list of conditions and the following
|
|
||||||
* disclaimer in the documentation and/or other materials provided
|
|
||||||
* with the distribution.
|
|
||||||
* * Neither the name of Bielefeld University nor the names of its
|
|
||||||
* contributors may be used to endorse or promote products derived
|
|
||||||
* from this software without specific prior written permission.
|
|
||||||
*
|
|
||||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
||||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
||||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
*********************************************************************/
|
|
||||||
|
|
||||||
/* Authors: Michael Goerner */
|
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/gripper.h>
|
|
||||||
#include <moveit/task_constructor/storage.h>
|
|
||||||
#include <moveit/task_constructor/task.h>
|
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
|
||||||
#include <moveit/robot_state/conversions.h>
|
|
||||||
#include <moveit/planning_pipeline/planning_pipeline.h>
|
|
||||||
|
|
||||||
#include <moveit/move_group_interface/move_group_interface.h>
|
|
||||||
|
|
||||||
#include <moveit_msgs/MotionPlanRequest.h>
|
|
||||||
#include <moveit_msgs/MotionPlanResponse.h>
|
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
|
||||||
|
|
||||||
Gripper::Gripper(std::string name)
|
|
||||||
: PropagatingEitherWay(name)
|
|
||||||
{
|
|
||||||
auto& p = properties();
|
|
||||||
p.declare<std::string>("eef", "name of end-effector group");
|
|
||||||
p.declare<std::string>("link", "name of link the eef is attached to");
|
|
||||||
p.declare<std::string>("named_target", "named target in eef group");
|
|
||||||
p.declare<std::string>("grasp_object", "name of grasp object");
|
|
||||||
}
|
|
||||||
|
|
||||||
void Gripper::init(const moveit::core::RobotModelConstPtr& robot_model)
|
|
||||||
{
|
|
||||||
PropagatingEitherWay::init(robot_model);
|
|
||||||
planner_ = Task::createPlanner(robot_model);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Gripper::setEndEffector(std::string eef){
|
|
||||||
setProperty("eef", eef);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Gripper::setAttachLink(std::string link){
|
|
||||||
setProperty("link", link);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Gripper::setFrom(std::string named_target){
|
|
||||||
restrictDirection(BACKWARD);
|
|
||||||
setProperty("named_target", named_target);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Gripper::setTo(std::string named_target){
|
|
||||||
restrictDirection(FORWARD);
|
|
||||||
setProperty("named_target", named_target);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Gripper::graspObject(std::string grasp_object){
|
|
||||||
setProperty("grasp_object", grasp_object);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Gripper::compute(const InterfaceState &state, planning_scene::PlanningScenePtr &scene,
|
|
||||||
robot_trajectory::RobotTrajectoryPtr &trajectory, double &cost, Direction dir){
|
|
||||||
scene = state.scene()->diff();
|
|
||||||
assert(scene->getRobotModel());
|
|
||||||
|
|
||||||
const auto& props = properties();
|
|
||||||
const std::string& eef = props.get<std::string>("eef");
|
|
||||||
std::string link = props.get<std::string>("link", "");
|
|
||||||
const std::string& named_target = props.get<std::string>("named_target");
|
|
||||||
const std::string& grasp_object = props.get<std::string>("grasp_object", "");
|
|
||||||
|
|
||||||
if(!mgi_){
|
|
||||||
assert(scene->getRobotModel()->hasEndEffector(eef) && "no end effector with that name defined in srdf");
|
|
||||||
const moveit::core::JointModelGroup* jmg= scene->getRobotModel()->getEndEffector(eef);
|
|
||||||
const std::string group_name= jmg->getName();
|
|
||||||
mgi_= std::make_shared<moveit::planning_interface::MoveGroupInterface>(group_name);
|
|
||||||
|
|
||||||
if( link.empty() ){
|
|
||||||
link= jmg->getEndEffectorParentGroup().second;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
mgi_->setNamedTarget(named_target);
|
|
||||||
|
|
||||||
::planning_interface::MotionPlanRequest req;
|
|
||||||
mgi_->constructMotionPlanRequest(req);
|
|
||||||
|
|
||||||
if( !grasp_object.empty() ){
|
|
||||||
scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object, mgi_->getLinkNames(), true);
|
|
||||||
}
|
|
||||||
|
|
||||||
::planning_interface::MotionPlanResponse res;
|
|
||||||
if(!planner_->generatePlan(scene, req, res))
|
|
||||||
return false;
|
|
||||||
|
|
||||||
trajectory = res.trajectory_;
|
|
||||||
if (dir == BACKWARD) trajectory->reverse();
|
|
||||||
|
|
||||||
scene->setCurrentState(trajectory->getLastWayPoint());
|
|
||||||
|
|
||||||
// attach object
|
|
||||||
if( !grasp_object.empty() ){
|
|
||||||
moveit_msgs::AttachedCollisionObject obj;
|
|
||||||
obj.link_name= link;
|
|
||||||
obj.object.id= grasp_object;
|
|
||||||
scene->processAttachedCollisionObjectMsg(obj);
|
|
||||||
}
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Gripper::computeForward(const InterfaceState &from){
|
|
||||||
planning_scene::PlanningScenePtr to;
|
|
||||||
robot_trajectory::RobotTrajectoryPtr trajectory;
|
|
||||||
double cost = 0;
|
|
||||||
|
|
||||||
if (!compute(from, to, trajectory, cost, FORWARD))
|
|
||||||
return false;
|
|
||||||
sendForward(from, InterfaceState(to), SubTrajectory(trajectory, cost));
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Gripper::computeBackward(const InterfaceState &to)
|
|
||||||
{
|
|
||||||
planning_scene::PlanningScenePtr from;
|
|
||||||
robot_trajectory::RobotTrajectoryPtr trajectory;
|
|
||||||
double cost = 0;
|
|
||||||
|
|
||||||
if (!compute(to, from, trajectory, cost, BACKWARD))
|
|
||||||
return false;
|
|
||||||
sendBackward(InterfaceState(from), to, SubTrajectory(trajectory, cost));
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
} } }
|
|
||||||
@ -1,103 +0,0 @@
|
|||||||
/*********************************************************************
|
|
||||||
* Software License Agreement (BSD License)
|
|
||||||
*
|
|
||||||
* Copyright (c) 2017, Hamburg University
|
|
||||||
* All rights reserved.
|
|
||||||
*
|
|
||||||
* Redistribution and use in source and binary forms, with or without
|
|
||||||
* modification, are permitted provided that the following conditions
|
|
||||||
* are met:
|
|
||||||
*
|
|
||||||
* * Redistributions of source code must retain the above copyright
|
|
||||||
* notice, this list of conditions and the following disclaimer.
|
|
||||||
* * Redistributions in binary form must reproduce the above
|
|
||||||
* copyright notice, this list of conditions and the following
|
|
||||||
* disclaimer in the documentation and/or other materials provided
|
|
||||||
* with the distribution.
|
|
||||||
* * Neither the name of Bielefeld University nor the names of its
|
|
||||||
* contributors may be used to endorse or promote products derived
|
|
||||||
* from this software without specific prior written permission.
|
|
||||||
*
|
|
||||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
||||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
||||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
||||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
||||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
||||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
||||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
||||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
||||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
||||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
||||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
||||||
* POSSIBILITY OF SUCH DAMAGE.
|
|
||||||
*********************************************************************/
|
|
||||||
|
|
||||||
/* Authors: Michael Goerner */
|
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/move.h>
|
|
||||||
#include <moveit/task_constructor/storage.h>
|
|
||||||
#include <moveit/task_constructor/task.h>
|
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
|
||||||
#include <moveit/robot_state/conversions.h>
|
|
||||||
#include <moveit/planning_pipeline/planning_pipeline.h>
|
|
||||||
|
|
||||||
#include <moveit/move_group_interface/move_group_interface.h>
|
|
||||||
|
|
||||||
#include <moveit_msgs/MotionPlanRequest.h>
|
|
||||||
#include <moveit_msgs/MotionPlanResponse.h>
|
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
|
||||||
|
|
||||||
Move::Move(std::string name)
|
|
||||||
: Connecting(name)
|
|
||||||
{
|
|
||||||
auto& p = properties();
|
|
||||||
p.declare<double>("timeout", 5.0, "planning timeout");
|
|
||||||
p.declare<std::string>("group", "name of planning group");
|
|
||||||
p.declare<std::string>("planner", "", "planner id");
|
|
||||||
}
|
|
||||||
|
|
||||||
void Move::init(const moveit::core::RobotModelConstPtr& robot_model)
|
|
||||||
{
|
|
||||||
Connecting::init(robot_model);
|
|
||||||
planner_ = Task::createPlanner(robot_model);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Move::setGroup(const std::string& group){
|
|
||||||
setProperty("group", group);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Move::setPlannerId(const std::string& planner){
|
|
||||||
setProperty("planner", planner);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Move::setTimeout(double timeout){
|
|
||||||
setProperty("timeout", timeout);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Move::compute(const InterfaceState &from, const InterfaceState &to) {
|
|
||||||
const auto& props = properties();
|
|
||||||
moveit::planning_interface::MoveGroupInterface mgi(props.get<std::string>("group"));
|
|
||||||
mgi.setJointValueTarget(to.scene()->getCurrentState());
|
|
||||||
|
|
||||||
const std::string planner_id = props.get<std::string>("planner");
|
|
||||||
if( !planner_id.empty() )
|
|
||||||
mgi.setPlannerId(planner_id);
|
|
||||||
mgi.setPlanningTime(props.get<double>("timeout"));
|
|
||||||
|
|
||||||
::planning_interface::MotionPlanRequest req;
|
|
||||||
mgi.constructMotionPlanRequest(req);
|
|
||||||
|
|
||||||
ros::Duration(4.0).sleep(); // TODO: get rid of this!
|
|
||||||
::planning_interface::MotionPlanResponse res;
|
|
||||||
if(!planner_->generatePlan(from.scene(), req, res))
|
|
||||||
return false;
|
|
||||||
|
|
||||||
// finish stage
|
|
||||||
connect(from, to, SubTrajectory(res.trajectory_));
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
} } }
|
|
||||||
Loading…
Reference in New Issue
Block a user