From a044b719ac144181e601df337ae318d7face7a09 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 8 Apr 2018 01:08:59 +0200 Subject: [PATCH] removed old stages: Move, Gripper, CartesianPositionMotion --- core/demo/plan_pick_ur5.cpp | 111 +++----- .../stages/cartesian_position_motion.h | 79 ------ .../moveit/task_constructor/stages/gripper.h | 74 ----- .../moveit/task_constructor/stages/move.h | 60 ---- core/src/stages/CMakeLists.txt | 8 - core/src/stages/cartesian_position_motion.cpp | 265 ------------------ core/src/stages/gripper.cpp | 164 ----------- core/src/stages/move.cpp | 103 ------- 8 files changed, 36 insertions(+), 828 deletions(-) delete mode 100644 core/include/moveit/task_constructor/stages/cartesian_position_motion.h delete mode 100644 core/include/moveit/task_constructor/stages/gripper.h delete mode 100644 core/include/moveit/task_constructor/stages/move.h delete mode 100644 core/src/stages/cartesian_position_motion.cpp delete mode 100644 core/src/stages/gripper.cpp delete mode 100644 core/src/stages/move.cpp diff --git a/core/demo/plan_pick_ur5.cpp b/core/demo/plan_pick_ur5.cpp index 61ddb236..3cd1919e 100644 --- a/core/demo/plan_pick_ur5.cpp +++ b/core/demo/plan_pick_ur5.cpp @@ -1,14 +1,12 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include - #include 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("current state"); + initial_stage = initial.get(); + t.add(std::move(initial)); - { - auto initial = std::make_unique("current state"); - initial_stage = initial.get(); - t.add(std::move(initial)); - } + // planner used for connect + auto pipeline = std::make_shared(); + pipeline->setTimeout(8.0); + pipeline->setPlannerId("RRTConnectkConfigDefault"); + // connect to pick + stages::Connect::GroupPlannerVector planners = {{"arm", pipeline}, {"gripper", pipeline}}; + auto connect = std::make_unique("connect", planners); + connect->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(connect)); - { - auto move = std::make_unique("open gripper"); - move->properties().configureInitFrom(Stage::PARENT); - move->setTo("open"); - t.add(std::move(move)); - } + // grasp generator + auto grasp_generator = std::make_unique(); + 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("move to pre-grasp"); - move->properties().configureInitFrom(Stage::PARENT); - move->setTimeout(8.0); - t.add(std::move(move)); - } + auto pick = std::make_unique(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("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("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("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("grasp"); - move->properties().configureInitFrom(Stage::PARENT); - move->setTo("closed"); - move->graspObject("object"); - t.add(std::move(move)); - } - - { - auto move = std::make_unique("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(); diff --git a/core/include/moveit/task_constructor/stages/cartesian_position_motion.h b/core/include/moveit/task_constructor/stages/cartesian_position_motion.h deleted file mode 100644 index d6e1094a..00000000 --- a/core/include/moveit/task_constructor/stages/cartesian_position_motion.h +++ /dev/null @@ -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 - -#include -#include - -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_; -}; - -} } } diff --git a/core/include/moveit/task_constructor/stages/gripper.h b/core/include/moveit/task_constructor/stages/gripper.h deleted file mode 100644 index 94ce77dc..00000000 --- a/core/include/moveit/task_constructor/stages/gripper.h +++ /dev/null @@ -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 - -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_; -}; - -} } } diff --git a/core/include/moveit/task_constructor/stages/move.h b/core/include/moveit/task_constructor/stages/move.h deleted file mode 100644 index e394f1bd..00000000 --- a/core/include/moveit/task_constructor/stages/move.h +++ /dev/null @@ -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 - -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_; -}; - -} } } diff --git a/core/src/stages/CMakeLists.txt b/core/src/stages/CMakeLists.txt index 7ffdc1fe..ff3e06be 100644 --- a/core/src/stages/CMakeLists.txt +++ b/core/src/stages/CMakeLists.txt @@ -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}) diff --git a/core/src/stages/cartesian_position_motion.cpp b/core/src/stages/cartesian_position_motion.cpp deleted file mode 100644 index 216bec7e..00000000 --- a/core/src/stages/cartesian_position_motion.cpp +++ /dev/null @@ -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 -#include - -#include -#include -#include -#include - -#include - -#include - -namespace moveit { namespace task_constructor { namespace stages { - -CartesianPositionMotion::CartesianPositionMotion(std::string name) -: PropagatingEitherWay(name) -{ - auto& p = properties(); - p.declare("group", "name of planning group"); - p.declare("link", "name of link used for IK"); - p.declare("min_distance", "minimum distance to move"); - p.declare("max_distance", "maximum distance to move"); - p.declare("step_size", 0.005); - p.declare("towards", "target point of motion"); - p.declare("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(*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("group"); - const std::string& link = props.get("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 trajectory_steps; - bool succeeded= false; - - if( mode_ == CartesianPositionMotion::MODE_TOWARDS ){ - const geometry_msgs::PointStamped& towards = props.get("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("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("min_distance"); - } - else if( mode_ == CartesianPositionMotion::MODE_ALONG ){ - const geometry_msgs::Vector3Stamped& along = props.get("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("max_distance"), - props.get("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("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_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("group"); - const std::string& link = props.get("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("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 trajectory_steps; - - double achieved_distance= robot_state.computeCartesianPath( - jmg, - trajectory_steps, - link_model, - direction, - true, /* global frame */ - props.get("max_distance"), - props.get("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("min_distance"); - - if(succeeded){ - robot_trajectory::RobotTrajectoryPtr traj= std::make_shared(robot_state.getRobotModel(), jmg); - for( auto& tp : trajectory_steps ) - traj->addPrefixWayPoint(tp, 0.0); - sendBackward(InterfaceState(result_scene), to, SubTrajectory(traj)); - } - - return succeeded; -} - -} } } diff --git a/core/src/stages/gripper.cpp b/core/src/stages/gripper.cpp deleted file mode 100644 index 1c039f80..00000000 --- a/core/src/stages/gripper.cpp +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include - -#include - -#include -#include - -namespace moveit { namespace task_constructor { namespace stages { - -Gripper::Gripper(std::string name) - : PropagatingEitherWay(name) -{ - auto& p = properties(); - p.declare("eef", "name of end-effector group"); - p.declare("link", "name of link the eef is attached to"); - p.declare("named_target", "named target in eef group"); - p.declare("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("eef"); - std::string link = props.get("link", ""); - const std::string& named_target = props.get("named_target"); - const std::string& grasp_object = props.get("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(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; -} - -} } } diff --git a/core/src/stages/move.cpp b/core/src/stages/move.cpp deleted file mode 100644 index 7400128c..00000000 --- a/core/src/stages/move.cpp +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include - -#include - -#include -#include - -namespace moveit { namespace task_constructor { namespace stages { - -Move::Move(std::string name) - : Connecting(name) -{ - auto& p = properties(); - p.declare("timeout", 5.0, "planning timeout"); - p.declare("group", "name of planning group"); - p.declare("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("group")); - mgi.setJointValueTarget(to.scene()->getCurrentState()); - - const std::string planner_id = props.get("planner"); - if( !planner_id.empty() ) - mgi.setPlannerId(planner_id); - mgi.setPlanningTime(props.get("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; -} - -} } }