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/stages/current_state.h>
|
||||
#include <moveit/task_constructor/stages/gripper.h>
|
||||
#include <moveit/task_constructor/stages/move.h>
|
||||
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
||||
#include <moveit/task_constructor/stages/cartesian_position_motion.h>
|
||||
#include <moveit/task_constructor/stages/compute_ik.h>
|
||||
#include <moveit/task_constructor/stages/simple_grasp.h>
|
||||
#include <moveit/task_constructor/stages/pick.h>
|
||||
#include <moveit/task_constructor/stages/connect.h>
|
||||
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
@ -40,82 +38,45 @@ int main(int argc, char** argv){
|
||||
spawnObject();
|
||||
|
||||
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;
|
||||
auto initial = std::make_unique<stages::CurrentState>("current state");
|
||||
initial_stage = initial.get();
|
||||
t.add(std::move(initial));
|
||||
|
||||
{
|
||||
auto initial = std::make_unique<stages::CurrentState>("current state");
|
||||
initial_stage = initial.get();
|
||||
t.add(std::move(initial));
|
||||
}
|
||||
// planner used for connect
|
||||
auto pipeline = std::make_shared<solvers::PipelinePlanner>();
|
||||
pipeline->setTimeout(8.0);
|
||||
pipeline->setPlannerId("RRTConnectkConfigDefault");
|
||||
// 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));
|
||||
|
||||
{
|
||||
auto move = std::make_unique<stages::Gripper>("open gripper");
|
||||
move->properties().configureInitFrom(Stage::PARENT);
|
||||
move->setTo("open");
|
||||
t.add(std::move(move));
|
||||
}
|
||||
// grasp generator
|
||||
auto grasp_generator = std::make_unique<stages::SimpleGrasp>();
|
||||
grasp_generator->setIKFrame(Eigen::Translation3d(.03,0,0), "s_model_tool0");
|
||||
grasp_generator->setMaxIKSolutions(8);
|
||||
grasp_generator->setAngleDelta(.2);
|
||||
grasp_generator->setPreGraspPose("open");
|
||||
grasp_generator->setGraspPose("closed");
|
||||
grasp_generator->setMonitoredStage(initial_stage);
|
||||
|
||||
{
|
||||
auto move = std::make_unique<stages::Move>("move to pre-grasp");
|
||||
move->properties().configureInitFrom(Stage::PARENT);
|
||||
move->setTimeout(8.0);
|
||||
t.add(std::move(move));
|
||||
}
|
||||
auto pick = std::make_unique<stages::Pick>(std::move(grasp_generator));
|
||||
pick->setProperty("eef", std::string("gripper"));
|
||||
pick->setProperty("object", std::string("object"));
|
||||
geometry_msgs::TwistStamped approach;
|
||||
approach.header.frame_id = "s_model_tool0";
|
||||
approach.twist.linear.x = 1.0;
|
||||
pick->setApproachMotion(approach, 0.03, 0.1);
|
||||
|
||||
{
|
||||
auto move = std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose");
|
||||
// move->addSolutionCallback(std::bind(&Introspection::publishSolution, &t.introspection(), std::placeholders::_1));
|
||||
move->properties().configureInitFrom(Stage::PARENT);
|
||||
move->setMinMaxDistance(.03, 0.1);
|
||||
move->setCartesianStepSize(0.02);
|
||||
geometry_msgs::TwistStamped lift;
|
||||
lift.header.frame_id = "world";
|
||||
lift.twist.linear.z = 1.0;
|
||||
pick->setLiftMotion(lift, 0.03, 0.05);
|
||||
|
||||
geometry_msgs::PointStamped target;
|
||||
target.header.frame_id= "object";
|
||||
move->towards(target);
|
||||
t.add(std::move(move));
|
||||
}
|
||||
|
||||
{
|
||||
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));
|
||||
}
|
||||
t.add(std::move(pick));
|
||||
|
||||
try {
|
||||
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/pick.h
|
||||
|
||||
${PROJECT_INCLUDE}/stages/cartesian_position_motion.h
|
||||
${PROJECT_INCLUDE}/stages/gripper.h
|
||||
${PROJECT_INCLUDE}/stages/move.h
|
||||
|
||||
modify_planning_scene.cpp
|
||||
fix_collision_objects.cpp
|
||||
|
||||
@ -34,10 +30,6 @@ add_library(${PROJECT_NAME}_stages
|
||||
|
||||
simple_grasp.cpp
|
||||
pick.cpp
|
||||
|
||||
cartesian_position_motion.cpp
|
||||
gripper.cpp
|
||||
move.cpp
|
||||
)
|
||||
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