removed old stages: Move, Gripper, CartesianPositionMotion

This commit is contained in:
Robert Haschke 2018-04-08 01:08:59 +02:00
parent b22217deab
commit a044b719ac
8 changed files with 36 additions and 828 deletions

View File

@ -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");
initial_stage = initial.get();
t.add(std::move(initial));
{ // planner used for connect
auto initial = std::make_unique<stages::CurrentState>("current state"); auto pipeline = std::make_shared<solvers::PipelinePlanner>();
initial_stage = initial.get(); pipeline->setTimeout(8.0);
t.add(std::move(initial)); 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));
{ // grasp generator
auto move = std::make_unique<stages::Gripper>("open gripper"); 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->setTo("open"); 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::Move>("move to pre-grasp"); pick->setProperty("eef", std::string("gripper"));
move->properties().configureInitFrom(Stage::PARENT); pick->setProperty("object", std::string("object"));
move->setTimeout(8.0); geometry_msgs::TwistStamped approach;
t.add(std::move(move)); approach.header.frame_id = "s_model_tool0";
} approach.twist.linear.x = 1.0;
pick->setApproachMotion(approach, 0.03, 0.1);
{ geometry_msgs::TwistStamped lift;
auto move = std::make_unique<stages::CartesianPositionMotion>("proceed to grasp pose"); lift.header.frame_id = "world";
// move->addSolutionCallback(std::bind(&Introspection::publishSolution, &t.introspection(), std::placeholders::_1)); lift.twist.linear.z = 1.0;
move->properties().configureInitFrom(Stage::PARENT); pick->setLiftMotion(lift, 0.03, 0.05);
move->setMinMaxDistance(.03, 0.1);
move->setCartesianStepSize(0.02);
geometry_msgs::PointStamped target; t.add(std::move(pick));
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));
}
try { try {
t.plan(); t.plan();

View File

@ -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_;
};
} } }

View File

@ -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_;
};
} } }

View File

@ -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_;
};
} } }

View File

@ -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})

View File

@ -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;
}
} } }

View File

@ -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;
}
} } }

View File

@ -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;
}
} } }