Skip to content
Snippets Groups Projects
Commit fed861f9 authored by KingMaZito's avatar KingMaZito
Browse files

some cleaning, bug fixing, base drops now possible

parent 70d556b1
Branches
No related tags found
No related merge requests found
Pipeline #14955 failed
Stage: build
Show whitespace changes
Inline Side-by-side
Showing
with 67 additions and 2241 deletions
.catkin_tools/profiles/default/devel_collisions.txt
+ 1
− 1
View file @ fed861f9
/home/matteo/ws_panda/devel/./cmake.lock 42
/home/matteo/reachability/devel/./cmake.lock 21925
/home/matteo/reachability/devel/./cmake.lock 23953
/home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
src/franka_description/robots/ceti_double.urdf.xacro
+ 20
− 10
View file @ fed861f9
......@@ -7,36 +7,45 @@
<xacro:include filename="panda_arm.xacro"/>
<xacro:include filename="hand.xacro"/>
<xacro:arg name="scene" default='dummy.yaml'/>
<link name="world" />
<xacro:property name="yaml_file" value="$(find mtc)/results/dummy.yaml" />
<xacro:property name="yaml_file" value="$(find mtc)/results/dummy/-151902566.yaml" />
<xacro:property name="props" value="${xacro.load_yaml(yaml_file)}" />
<xacro:property name="x1" value="${props['objects'][8]['pos']['x']}" />
<xacro:property name="y1" value="${props['objects'][8]['pos']['y']}" />
<xacro:property name="z1" value="${props['objects'][8]['pos']['z']}" />
<xacro:property name="sx1" value="${props['objects'][8]['size']['length']}" />
<xacro:property name="sy1" value="${props['objects'][8]['size']['width']}" />
<xacro:property name="sz1" value="${props['objects'][8]['size']['height']}" />
<xacro:property name="rpy1" value="${props['objects'][8]['rpy']['r']} ${props['objects'][8]['rpy']['p']} ${props['objects'][8]['rpy']['y']}" />
<xacro:property name="x2" value="${props['objects'][17]['pos']['x']}" />
<xacro:property name="y2" value="${props['objects'][17]['pos']['y']}" />
<xacro:property name="z2" value="${props['objects'][17]['pos']['z']}" />
<xacro:property name="rpy2" value="${props['objects'][17]['rpy']['r']} ${props['objects'][17]['rpy']['p']} ${props['objects'][17]['rpy']['y']}" />
<xacro:property name="sx2" value="${props['objects'][17]['size']['length']}" />
<xacro:property name="sy2" value="${props['objects'][17]['size']['width']}" />
<xacro:property name="sz2" value="${props['objects'][17]['size']['height']}" />
<!-- Dummy Link -->
<joint name="base_joint1" type="fixed">
<parent link="world" />
<child link="base_1" />
<origin xyz="${x1} ${y1} ${z1 * 0.5}" rpy="${rpy1}"/>
<origin xyz="${x1} ${y1} ${(z1* 0.5) + (sz1 * 0.25)}" rpy="${rpy1}"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="base_joint2" type="fixed">
<parent link="world" />
<child link="base_2" />
<origin xyz="${x2} ${y2} ${z2 * 0.5}" rpy="${rpy2}"/>
<origin xyz="${x2} ${y2} ${(z2* 0.5) + (sz2 * 0.25)}" rpy="${rpy2}"/>
<axis xyz="0 0 1"/>
</joint>
......@@ -45,7 +54,7 @@
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.8 0.8 ${z1}" />
<box size="${sx1} ${sy1} ${z1 + (sz1 * 0.5)}" />
</geometry>
<material name="White">
<color rgba="1.0 1.0 1.0 1.0"/>
......@@ -54,7 +63,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.8 0.8 ${z1}" />
<box size="${sx1} ${sy1} ${z1 + (sz1 * 0.5)}" />
</geometry>
</collision>
</link>
......@@ -63,7 +72,7 @@
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.8 0.8 ${z2}" />
<box size="${sx2} ${sy2} ${z2 + (sz2 * 0.5)}" />
</geometry>
<material name="White">
<color rgba="1.0 1.0 1.0 1.0"/>
......@@ -72,18 +81,19 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.8 0.8 ${z2}" />
<box size="${sx2} ${sy2} ${z2 + (sz2 * 0.5)}" />
</geometry>
</collision>
</link>
<!-- right arm with gripper -->
<xacro:panda_arm arm_id="$(arg arm_id_1)" connected_to="base_1" xyz="-0.22 0 ${z1 * 0.5}" />
<xacro:panda_arm arm_id="$(arg arm_id_1)" connected_to="base_1" xyz="-0.22 0 ${(z1* 0.5) + (sz1 * 0.25)}" />
<xacro:hand ns="$(arg arm_id_1)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_1)_link8" />
<!-- left arm with gripper -->
<xacro:panda_arm arm_id="$(arg arm_id_2)" connected_to="base_2" xyz="-0.22 0 ${z2 * 0.5}" />
<xacro:panda_arm arm_id="$(arg arm_id_2)" connected_to="base_2" xyz="-0.22 0 ${(z2* 0.5) + (sz2 * 0.25)}" />
<xacro:hand ns="$(arg arm_id_2)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_2)_link8"/>
</robot>
src/mtc/CMakeLists.txt
+ 0
− 43
View file @ fed861f9
......@@ -38,14 +38,10 @@ find_package(catkin REQUIRED COMPONENTS
trajectory_msgs
pcl_ros
yaml_to_mtc
)
# Catkin
catkin_package(
LIBRARIES
moveit_grasps
moveit_grasps_filter
CATKIN_DEPENDS
geometry_msgs
message_runtime
......@@ -102,42 +98,7 @@ src/impl/collision_helper.cpp
# Grasp Library
add_library(moveit_grasps
src/grasp_candidate.cpp
src/grasp_data.cpp
src/grasp_generator.cpp
src/grasp_scorer.cpp
src/two_finger_grasp_data.cpp
src/two_finger_grasp_generator.cpp
src/two_finger_grasp_scorer.cpp
)
target_link_libraries(moveit_grasps
${catkin_LIBRARIES} ${Boost_LIBRARIES}
)
# Grasp Filter Library
add_library(moveit_grasps_filter
src/grasp_filter.cpp
src/two_finger_grasp_filter.cpp
src/grasp_planner.cpp
)
target_link_libraries(moveit_grasps_filter
moveit_grasps
${catkin_LIBRARIES}
${Boost_LIBRARIES}
)
set_target_properties(moveit_grasps_filter PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") # for threading
set_target_properties(moveit_grasps_filter PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")
add_executable(mtc2taskspace src/mtc2taskspace.cpp)
add_executable(moveit_grasps_grasp_pipeline src/grasp_pipeline.cpp)
add_dependencies(base_routine ${base_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
......@@ -145,10 +106,6 @@ add_dependencies(cell_routine ${cell_routine_EXPORTED_TARGETS} ${catkin_EXPORTED
add_dependencies(mtc2taskspace ${mtc2taskspace_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(moveit_grasps_grasp_pipeline
moveit_grasps moveit_grasps_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES} yaml-cpp
)
target_link_libraries(base_routine
${catkin_LIBRARIES}
${OCTOMAP_LIBRARIES}
......
src/mtc/descriptions/dummy.yaml
+ 2
− 2
View file @ fed861f9
task:
groups:
robot_x:
panda_arm1:
- -0.300000 -0.700000 0.890000
- -0.300000 -0.600000 0.890000
- -0.200000 -0.700000 0.890000
......@@ -31,7 +31,7 @@ task:
- -0.200000 0.700000 0.890000
- 0.100000 0.600000 0.890000
- 0.100000 0.700000 0.890000
robot_y:
panda_arm2:
- 0.100000 1.010000 0.890000
- 0.100000 1.110000 0.890000
- 0.100000 1.210000 0.890000
......
src/mtc/include/grasp_candidate.h deleted 100644 → 0
+ 0
− 142
View file @ 70d556b1
#ifndef MOVEIT_GRASPS__GRASP_CANDIDATE_
#define MOVEIT_GRASPS__GRASP_CANDIDATE_
// ROS
#include <ros/ros.h>
#include <moveit_msgs/Grasp.h>
// Grasping
#include "../include/grasp_data.h"
// MoveIt
#include <moveit/robot_state/robot_state.h>
#include <moveit_msgs/Grasp.h>
namespace moveit_grasps
{
typedef std::vector<std::vector<moveit::core::RobotStatePtr> > GraspTrajectories;
enum GraspTrajectorySegments
{
APPROACH = 0,
LIFT = 1,
RETREAT = 2
};
struct GraspFilterCode
{
enum
{
NOT_FILTERED = 0,
GRASP_FILTERED_BY_IK, // Ik solution at grasp failed
GRASP_FILTERED_BY_CUTTING_PLANE, // grasp pose is in an unreachable part of the environment (eg: behind a wall)
GRASP_FILTERED_BY_ORIENTATION, // grasp pose is not desireable
GRASP_FILTERED_BY_IK_CLOSED, // ik solution was fine with grasp opened, but failed with grasp closed
PREGRASP_FILTERED_BY_IK, // Ik solution before approach failed
GRASP_INVALID, // An error occured while processing the grasp
LAST // Used to track last value in the base class when inheriting
};
};
/**
* \brief Contains collected data for each potential grasp after it has been verified / filtered
* This includes the pregrasp and grasp IK solution
*/
class GraspCandidate
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
GraspCandidate(const moveit_msgs::Grasp& grasp, const GraspDataPtr& grasp_data, const Eigen::Isometry3d& cuboid_pose);
bool getPreGraspState(moveit::core::RobotStatePtr& robot_state);
bool getGraspStateOpen(moveit::core::RobotStatePtr& robot_state);
bool getGraspStateOpenEEOnly(moveit::core::RobotStatePtr& robot_state);
bool getGraspStateClosed(moveit::core::RobotStatePtr& robot_state);
bool getGraspStateClosedEEOnly(moveit::core::RobotStatePtr& robot_state);
virtual bool isValid();
moveit_msgs::Grasp grasp_;
/*# Contents of moveit_msgs::Grasp for reference
# A name for this grasp
string id
# The internal posture of the hand for the pre-grasp
# only positions are used
trajectory_msgs/JointTrajectory pre_grasp_posture
# The internal posture of the hand for the grasp
# positions and efforts are used
trajectory_msgs/JointTrajectory grasp_posture
# The position of the end-effector for the grasp. This is the pose of
# the "parent_link" of the end-effector, not actually the pose of any
# link *in* the end-effector. Typically this would be the pose of the
# most distal wrist link before the hand (end-effector) links began.
geometry_msgs/PoseStamped grasp_pose
# The estimated probability of success for this grasp, or some other
# measure of how "good" it is.
float64 grasp_quality
# The approach direction to take before picking an object
GripperTranslation pre_grasp_approach
{
# defines a translation for the gripper, used in pickup or place tasks
# for example for lifting an object off a table or approaching the table for placing
# the direction of the translation
geometry_msgs/Vector3Stamped direction
# the desired translation distance
float32 desired_distance
# the min distance that must be considered feasible before the
# grasp is even attempted
float32 min_distance
}
# The retreat direction to take after a grasp has been completed (object is attached)
GripperTranslation post_grasp_retreat
# The retreat motion to perform when releasing the object; this information
# is not necessary for the grasp itself, but when releasing the object,
# the information will be necessary. The grasp used to perform a pickup
# is returned as part of the result, so this information is available for
# later use.
GripperTranslation post_place_retreat
# the maximum contact force to use while grasping (<=0 to disable)
float32 max_contact_force
# an optional list of obstacles that we have semantic information about
# and that can be touched/pushed/moved in the course of grasping
string[] allowed_touch_objects
# The minimum opening that the fingers can have as it approaches an object
float64 min_finger_open_on_approach
*/
const GraspDataPtr grasp_data_;
// TODO(davetcoleman): possibly remove
Eigen::Isometry3d cuboid_pose_; // pose of original object to grasp
int grasp_filtered_code_; // All codes defined in enum
std::vector<double> grasp_ik_solution_;
std::vector<double> pregrasp_ik_solution_;
// Store pregrasp, grasp, lifted, and retreat trajectories
GraspTrajectories segmented_cartesian_traj_;
}; // class
typedef std::shared_ptr<GraspCandidate> GraspCandidatePtr;
} // namespace moveit_grasps
#endif
src/mtc/include/grasp_data.h deleted 100644 → 0
+ 0
− 102
View file @ 70d556b1
#ifndef MOVEIT_GRASPS__GRASP_DATA_H_
#define MOVEIT_GRASPS__GRASP_DATA_H_
// Ros
#include <ros/node_handle.h>
// Msgs
#include <geometry_msgs/Pose.h>
#include <trajectory_msgs/JointTrajectory.h>
// MoveIt
#include <moveit/macros/class_forward.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_model/link_model.h>
// moveit grasps
#include "suction_voxel_matrix.h"
namespace moveit_grasps
{
MOVEIT_CLASS_FORWARD(GraspData);
// Map various arms to end effector grasp datas
typedef std::map<const robot_model::JointModelGroup*, moveit_grasps::GraspDataPtr> GraspDatas;
struct GraspData
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Loads grasp data from a yaml file (load from roslaunch)
* \param node handle - allows for namespacing
* \param end effector name - which side of a two handed robot to load data for. should correspond to SRDF EE names
*/
GraspData(const ros::NodeHandle& nh, const std::string& end_effector,
const moveit::core::RobotModelConstPtr& robot_model);
/**
* \brief Helper function for constructor
* \return true on success
*/
virtual bool loadGraspData(const ros::NodeHandle& nh, const std::string& end_effector);
/**
* \brief Alter a robot state so that the end effector corresponding to this grasp data is in pre-grasp state (OPEN)
* \param joint state of robot
* \return true on success
*/
bool setRobotStatePreGrasp(robot_state::RobotStatePtr& robot_state);
/**
* \brief Alter a robot state so that the end effector corresponding to this grasp data is in grasp state (CLOSED)
* \param joint state of robot
* \return true on success
*/
bool setRobotStateGrasp(robot_state::RobotStatePtr& robot_state);
/**
* \brief Alter a robot state so that the end effector corresponding to this grasp data is in a grasp posture
* \param joint state of robot
* \param posture - what state to set the end effector
* \return true on success
*/
bool setRobotState(robot_state::RobotStatePtr& robot_state, const trajectory_msgs::JointTrajectory& posture);
/**
* \brief Debug data to console
*/
virtual void print();
// The (possibly fictional) center point of a grasp
std::string tcp_name_;
Eigen::Isometry3d tcp_to_eef_mount_; // Convert generic grasp pose to the parent arm's eef_mount frame of reference
trajectory_msgs::JointTrajectory pre_grasp_posture_; // when the end effector is in "open" position
trajectory_msgs::JointTrajectory grasp_posture_; // when the end effector is in "close" position
std::string base_link_; // name of global frame with z pointing up
const robot_model::JointModelGroup* ee_jmg_; // this end effector
const robot_model::JointModelGroup* arm_jmg_; // the arm that attaches to this end effector
const robot_model::RobotModelConstPtr robot_model_;
// Duplicate end effector data copied from RobotModel
// the last link in the kinematic chain before the end effector, e.g. "/gripper_roll_link" class
const robot_model::LinkModel* parent_link_;
int angle_resolution_; // generate grasps at increments of: angle_resolution * pi / 180
double grasp_resolution_;
double grasp_depth_resolution_; // generate grasps at this depth resolution along grasp_max_depth_
double grasp_min_depth_; // minimum amount fingers must overlap object
double grasp_max_depth_; // Maximum distance from tip of end effector inwords that an object can be for a grasp
// grasp approach and retreat parameters
double approach_distance_desired_; // this is in addition to the grasp_max_depth
double retreat_distance_desired_; // this is in addition to the grasp_max_depth
double lift_distance_desired_;
double grasp_padding_on_approach_;
};
} // namespace moveit_grasps
#endif
src/mtc/include/grasp_filter.h deleted 100644 → 0
+ 0
− 391
View file @ 70d556b1
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, University of Colorado, Boulder
* 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 the Univ of CO, Boulder 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.
*********************************************************************/
/* Author: Dave Coleman <dave@picknik.ai>
Desc: Filters grasps based on kinematic feasibility and collision
*/
#ifndef MOVEIT_GRASPS__GRASP_FILTER_
#define MOVEIT_GRASPS__GRASP_FILTER_
// ROS
#include <ros/ros.h>
#include <tf_conversions/tf_eigen.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
// Grasping
#include "../include/grasp_generator.h"
#include "../include/grasp_candidate.h"
// Rviz
#include <moveit_visual_tools/moveit_visual_tools.h>
// MoveIt
#include <moveit/robot_state/robot_state.h>
#include <moveit_msgs/Grasp.h>
#include <moveit/kinematics_plugin_loader/kinematics_plugin_loader.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
// C++
#include <boost/thread.hpp>
#include <cmath>
namespace moveit_grasps
{
enum GraspParallelPlane
{
XY,
XZ,
YZ
};
/**
* \brief Contains information to filter grasps by a cutting plane
*/
struct CuttingPlane
{
Eigen::Isometry3d pose_;
GraspParallelPlane plane_;
int direction_;
CuttingPlane(Eigen::Isometry3d pose, GraspParallelPlane plane, int direction)
: pose_(pose), plane_(plane), direction_(direction)
{
}
};
typedef std::shared_ptr<CuttingPlane> CuttingPlanePtr;
/**
* \brief Contains information to filter grasps by orientation
*/
struct DesiredGraspOrientation
{
Eigen::Isometry3d pose_;
double max_angle_offset_;
DesiredGraspOrientation(Eigen::Isometry3d pose, double max_angle_offset)
: pose_(pose), max_angle_offset_(max_angle_offset)
{
}
};
typedef std::shared_ptr<DesiredGraspOrientation> DesiredGraspOrientationPtr;
/**
* \brief Struct for passing parameters to threads, for cleaner code
*/
struct IkThreadStruct
{
IkThreadStruct(std::vector<GraspCandidatePtr>& grasp_candidates, // the input
const planning_scene::PlanningScenePtr& planning_scene, Eigen::Isometry3d& link_transform,
std::size_t grasp_id, kinematics::KinematicsBaseConstPtr kin_solver,
const robot_state::RobotStatePtr& robot_state, double timeout, bool filter_pregrasp, bool visual_debug,
std::size_t thread_id, const std::string& grasp_target_object_id)
: grasp_candidates_(grasp_candidates)
, planning_scene_(planning_scene::PlanningScene::clone(planning_scene))
, link_transform_(link_transform)
, grasp_id(grasp_id)
, kin_solver_(kin_solver)
, robot_state_(std::make_shared<robot_state::RobotState>(*robot_state))
, timeout_(timeout)
, filter_pregrasp_(filter_pregrasp)
, visual_debug_(visual_debug)
, thread_id_(thread_id)
, grasp_target_object_id_(grasp_target_object_id)
{
}
std::vector<GraspCandidatePtr>& grasp_candidates_;
planning_scene::PlanningScenePtr planning_scene_;
Eigen::Isometry3d link_transform_;
std::size_t grasp_id;
kinematics::KinematicsBaseConstPtr kin_solver_;
robot_state::RobotStatePtr robot_state_;
double timeout_;
bool filter_pregrasp_;
bool visual_debug_;
std::size_t thread_id_;
// The name of the target grasp object to be used in the planning scene.
// NOTE: The ik filtering checks if the user has already added the grasp object
// to the planning scene. If it hasn't then GraspFilter will not modify the planning scene allowed collision matrix
std::string grasp_target_object_id_;
// Used within processing function
geometry_msgs::PoseStamped ik_pose_; // Set from grasp candidate
std::vector<double> ik_seed_state_;
};
typedef std::shared_ptr<IkThreadStruct> IkThreadStructPtr;
class GraspFilter
{
public:
// Constructor
GraspFilter(const robot_state::RobotStatePtr& robot_state,
const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools);
/**
* \brief Return grasps that are kinematically feasible
* \param grasp_candidates - all possible grasps that this will test. this vector is returned modified
* \param arm_jmg - the arm to solve the IK problem on
* \param filter_pregrasp -whether to also check ik feasibility for the pregrasp position
* \param target_object_id - The name of the target grasp object in the planning scene if it exists
* \return true on successful filter
*/
virtual bool filterGrasps(std::vector<GraspCandidatePtr>& grasp_candidates,
const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
const robot_model::JointModelGroup* arm_jmg, const moveit::core::RobotStatePtr& seed_state,
bool filter_pregrasp = false, const std::string& target_object_id = "");
virtual bool filterGrasps(std::vector<GraspCandidatePtr>& grasp_candidates,
const planning_scene::PlanningScenePtr& planning_scene,
const robot_model::JointModelGroup* arm_jmg, const moveit::core::RobotStatePtr& seed_state,
bool filter_pregrasp = false, const std::string& target_object_id = "");
/**
* \brief Return grasps that are kinematically feasible
* \param grasp_candidates - all possible grasps that this will test. this vector is returned modified
* \param arm_jmg - the arm to solve the IK problem on
* \param seed_state - A robot state to be used for IK. Ideally this will be close to the desired goal configuration.
* \param filter_pregrasp - Whether to also check ik feasibility for the pregrasp position
* \param visualize - visualize IK filtering
* \param target_object_id - The name of the target grasp object in the planning scene if it exists
* \return true on successful filter
*/
virtual std::size_t filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates,
const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
const robot_model::JointModelGroup* arm_jmg,
const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp,
bool visualize, const std::string& grasp_target_object_id = "");
virtual std::size_t filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates,
const planning_scene::PlanningScenePtr& planning_scene,
const robot_model::JointModelGroup* arm_jmg,
const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp,
bool visualize, const std::string& grasp_target_object_id = "");
/**
* \brief Print grasp filtering statistics
*/
virtual void printFilterStatistics(const std::vector<GraspCandidatePtr>& grasp_candidates) const;
/**
* \brief Method for checking part of the possible grasps list. MUST BE THREAD SAFE
*/
virtual bool processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct);
/**
* \brief Used for sorting an array of CandidateGrasps
* \return true if A is less than B
*/
static bool compareGraspScores(const GraspCandidatePtr& grasp_a, const GraspCandidatePtr& grasp_b)
{
// Determine if A or B has higher quality
return (grasp_a->grasp_.grasp_quality > grasp_b->grasp_.grasp_quality);
}
/**
* \brief Of an array of grasps, sort the valid ones from best score to worse score
* \return false if no grasps remain
*/
bool removeInvalidAndFilter(std::vector<GraspCandidatePtr>& grasp_candidates) const;
/**
* \brief Add a cutting plane filter for a shelf bin
* \return true on success
*/
bool addCuttingPlanesForBin(const Eigen::Isometry3d& world_to_bin, const Eigen::Isometry3d& bin_to_product,
const double& bin_width, const double& bin_height);
/* \brief Set the ACM entry to ignore collisions between the ee_link_names and the object in the planning_scene */
void setACMFingerEntry(const std::string& object_name, bool allowed, const std::vector<std::string>& ee_link_names,
const planning_scene::PlanningScenePtr& planning_scene);
protected:
/**
* \brief Filter grasps by cutting plane
* \param grasp_candidates - all possible grasps that this will test. this vector is returned modified
* \param filter_pose - pose of filter that will define cutting plane
* \param plane - the cutting plane (XY, XZ, or YZ)
* \param direction - which side of this plane to cut (+/- 1)
* \return true if grasp is filtered by operation
*/
bool filterGraspByPlane(GraspCandidatePtr& grasp_candidate, const Eigen::Isometry3d& filter_pose,
GraspParallelPlane plane, int direction) const;
/**
* \brief Filter grasps by desired orientation. Think of reaching into a small opening, you can only rotate your hand
* a tiny amount and still grasp an object. If there's empty space behind an object, grasps behind the object aren't
* removed by the cutting plane operations. We know we'll never get to them because they deviate too much from the
* desired grasping pose... straight in.
* \param grasp_candidates - a grasp candidate that this will test.
* \param desired_pose - the desired grasp pose ( using standard grasping orientation )
* \param max_angular_offset - maximum angle allowed between the grasp pose and the desired pose
* \return true if grasp is filtered by operation
*/
bool filterGraspByOrientation(GraspCandidatePtr& grasp_candidate, const Eigen::Isometry3d& desired_pose,
double max_angular_offset) const;
/**
* \brief Filter grasps by feasablity of the grasp pose.
* \param grasp_candidates - a grasp candidate that this will test
* \param grasp_ik_solution - the IK solution if one exists
* \param ik_thread_struct - a struct containing the planning_scene
* \return true if grasp is filtered by operation
*/
bool filterGraspByGraspIK(const GraspCandidatePtr& grasp_candidate, std::vector<double>& grasp_ik_solution,
const IkThreadStructPtr& ik_thread_struct) const;
/**
* \brief Filter grasps by feasablity of the pre grasp pose.
* \param grasp_candidates - a grasp candidate that this will test
* \param grasp_ik_solution - the IK solution if one exists
* \param ik_thread_struct - a struct containing the planning_scene
* \return true if grasp is filtered by operation
*/
bool filterGraspByPreGraspIK(const GraspCandidatePtr& grasp_candidate, std::vector<double>& pregrasp_ik_solution,
const IkThreadStructPtr& ik_thread_struct) const;
/**
* \brief Helper for the thread function to find IK solutions
* \return true on success
*/
bool findIKSolution(std::vector<double>& ik_solution, const IkThreadStructPtr& ik_thread_struct,
const GraspCandidatePtr& grasp_candidate,
const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const;
/**
* \brief add a cutting plane
* \param pose - pose describing the cutting plane
* \param plane - which plane to use as the cutting plane
* \param direction - on which side of the plane the grasps will be removed
*/
void addCuttingPlane(const Eigen::Isometry3d& pose, GraspParallelPlane plane, int direction);
/**
* \brief Show all cutting planes that are currently enables
* \return true on success
*/
bool visualizeCuttingPlanes();
/**
* \brief clear all cutting planes
*/
void clearCuttingPlanes();
/**
* \brief add a desired grasp orientation
* \param pose - the desired grasping pose
* \param max_angle_offset - maximum amount a generated grasp can deviate from the desired pose
*/
void addDesiredGraspOrientation(const Eigen::Isometry3d& pose, double max_angle_offset);
/**
* \brief clear all desired orientations
*/
void clearDesiredGraspOrientations();
/**
* \brief Show grasps after being filtered
* \return true on success
*/
bool visualizeGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates,
const moveit::core::JointModelGroup* arm_jmg);
/**
* \brief Show IK solutions of entire arm
* \return true on success
*/
bool visualizeIKSolutions(const std::vector<GraspCandidatePtr>& grasp_candidates,
const moveit::core::JointModelGroup* arm_jmg, double animation_speed);
/**
* \brief Show solutions of entire arm
* \return true on success
*/
bool visualizeCandidateGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates);
/**
* \brief Show an entire planning scene
*/
void publishPlanningScene(const planning_scene::PlanningScenePtr& ps) const;
protected:
// logging name
const std::string name_;
// Allow a writeable robot state
robot_state::RobotStatePtr robot_state_;
// Keep a robot state for every thread
std::vector<robot_state::RobotStatePtr> robot_states_;
// Threaded kinematic solvers
std::map<std::string, std::vector<kinematics::KinematicsBaseConstPtr> > kin_solvers_;
// Class for publishing stuff to rviz
moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;
// for rviz visualization of the planning scene
ros::Publisher planning_scene_publisher_;
// Number of degrees of freedom for the IK solver to find
std::size_t num_variables_;
// Time to allow IK solver to run
double solver_timeout_;
// Visualization levels
bool collision_verbose_;
bool statistics_verbose_;
double collision_verbose_speed_;
bool show_filtered_grasps_;
bool show_filtered_arm_solutions_;
bool show_cutting_planes_;
double show_filtered_arm_solutions_speed_;
double show_filtered_arm_solutions_pregrasp_speed_;
bool show_grasp_filter_collision_if_failed_;
// Shared node handle
ros::NodeHandle nh_;
// Cutting planes and orientation filter
std::vector<CuttingPlanePtr> cutting_planes_;
std::vector<DesiredGraspOrientationPtr> desired_grasp_orientations_;
}; // end of class
typedef std::shared_ptr<GraspFilter> GraspFilterPtr;
typedef std::shared_ptr<const GraspFilter> GraspFilterConstPtr;
} // namespace moveit_grasps
#endif
src/mtc/include/grasp_generator.h deleted 100644 → 0
+ 0
− 248
View file @ 70d556b1
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, University of Colorado, Boulder
* 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 the Univ of CO, Boulder 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.
*********************************************************************/
/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy
Desc: Generates geometric grasps for cuboids and blocks, not using physics or contact wrenches
*/
#ifndef MOVEIT_GRASPS__GRASP_GENERATOR_H_
#define MOVEIT_GRASPS__GRASP_GENERATOR_H_
// ROS
#include <ros/ros.h>
// TF
#include <tf_conversions/tf_eigen.h>
// Msgs
#include <geometry_msgs/PoseArray.h>
// MoveIt
#include <moveit_msgs/Grasp.h>
// geometric_shapes
#include <geometric_shapes/shape_operations.h>
#include <geometric_shapes/bodies.h>
// Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>
#include <eigen_conversions/eigen_msg.h>
// Visualization
#include <moveit_visual_tools/moveit_visual_tools.h>
// moveit_grasps
#include "grasp_candidate.h"
#include "grasp_scorer.h"
// bounding_box
//#include <bounding_box/bounding_box.h>
// C++
#include <cstdlib>
#include <string>
#include <cmath>
#include <limits>
#include "grasp_data.h"
namespace moveit_grasps
{
static const double RAD2DEG = 57.2957795;
static const double MIN_GRASP_DISTANCE = 0.001; // m between grasps
// Grasp axis orientation
enum grasp_axis_t
{
X_AXIS,
Y_AXIS,
Z_AXIS
};
class GraspGenerator
{
public:
// Eigen requires 128-bit alignment for the Eigen::Vector2d's array (of 2 doubles).
// With GCC, this is done with a attribute ((aligned(16))).
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor
*/
GraspGenerator(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, bool verbose = false);
/**
* \brief Create possible grasp positions around a cuboid
* \param cuboid_pose - centroid of object to grasp in world frame
* \param depth length of cuboid along local x-axis
* \param width length of cuboid along local y-axis
* \param height length of cuboid along local z-axis
* \param grasp_data data describing end effector
* \param grasp_candidate_config parameter for selectively enabling and disabling different grasp types
* \param grasp_candidates possible grasps generated
* \return true if successful
*/
virtual bool generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height,
const GraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates) = 0;
/**
* \brief Get the grasp direction vector relative to the world frame
* \param grasp
* \param name of parent link
* \return the approach direction
*/
static Eigen::Vector3d getPreGraspDirection(const moveit_msgs::Grasp& grasp, const std::string& ee_parent_link);
// static Eigen::Vector3d getPostGraspDirection(const moveit_msgs::Grasp &grasp, const std::string &ee_parent_link);
/**
* \brief Using an input grasp description, get the pregrasp pose. The pregrasp pose is the grasp pose translated
* backwards in the grasp frame away from the object being grasped.
* \param grasp
* \param name of parent link
* \return pregrasp pose
*/
static geometry_msgs::PoseStamped getPreGraspPose(const GraspCandidatePtr& grasp_candidate,
const std::string& ee_parent_link);
/**
* \brief Compute the pre-grasp, grasp, lift and retreat poses for a grasp candidate
* \param grasp_candidate - the grasp candidate
* \param grasp_waypoints - a reference to a vector that will be populated with the pre-grasp, grasp, lift and retreat
* poses in that order.
*/
static void getGraspWaypoints(const GraspCandidatePtr& grasp_candidate, EigenSTL::vector_Isometry3d& grasp_waypoints);
/**
* \brief Helper to convert a robot-specific grasp to an arrow pointed in the right direction
* \param grasp - the grasp to show
* \param arm - the planning group of the arm we want to display
* \return true on success
*/
void publishGraspArrow(const geometry_msgs::Pose& grasp, const GraspDataPtr& grasp_data,
const rviz_visual_tools::colors& color, double approach_length = 0.1);
/**
* \brief Getter for Verbose
*/
bool getVerbose()
{
return verbose_;
}
/**
* \brief Getter for ideal grasp pose
*/
Eigen::Isometry3d getIdealTCPGraspPose()
{
return ideal_grasp_pose_;
}
/**
* \brief Setter for ideal grasp pose for scoring
*/
void setIdealTCPGraspPose(Eigen::Isometry3d ideal_pose)
{
ideal_grasp_pose_ = ideal_pose;
}
/**
* \brief Setter for the roll pitch yall ideal grasp pose for scoring
*/
void setIdealTCPGraspPoseRPY(const std::vector<double>& ideal_grasp_orientation_rpy);
/**
* \brief Setter for grasp score weights
*/
void setGraspScoreWeights(const GraspScoreWeightsPtr& grasp_score_weights)
{
grasp_score_weights_ = std::make_shared<GraspScoreWeights>(*grasp_score_weights);
}
/**
* \brief Setter for grasp score weights
*/
const GraspScoreWeightsPtr& getGraspScoreWeights()
{
return grasp_score_weights_;
}
/**
* \brief Setter for Verbose
*/
void setVerbose(bool verbose)
{
verbose_ = verbose;
}
/**
* \brief Visualize animated grasps
* \return true on success
*/
bool visualizeAnimatedGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates,
const moveit::core::JointModelGroup* ee_jmg, double animation_speed);
// Ideal grasp pose for scoring purposes
Eigen::Isometry3d ideal_grasp_pose_;
protected:
// class for publishing stuff to rviz
moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;
// Display more output both in console
bool verbose_;
// Visual debug settings
bool show_prefiltered_grasps_;
double show_prefiltered_grasps_speed_;
// Shared node handle
ros::NodeHandle nh_;
// Transform from frame of box to global frame
Eigen::Isometry3d object_global_transform_;
double min_grasp_distance_, max_grasp_distance_;
Eigen::Vector3d min_translations_, max_translations_;
GraspScoreWeightsPtr grasp_score_weights_;
}; // end of class
typedef std::shared_ptr<GraspGenerator> GraspGeneratorPtr;
typedef std::shared_ptr<const GraspGenerator> GraspGeneratorConstPtr;
} // namespace moveit_grasps
#endif
src/mtc/include/grasp_planner copy.h deleted 100644 → 0
+ 0
− 163
View file @ 70d556b1
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, University of Colorado, Boulder
* 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 the Univ of CO, Boulder 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.
*********************************************************************/
/* Author: Dave Coleman <dave@picknik.ai>
Desc: Find the approach, lift, and retreat path for a candidate grasp (if a valid one exists)
*/
#ifndef MOVEIT_GRASPS__GRASP_PLANNER_
#define MOVEIT_GRASPS__GRASP_PLANNER_
// ROS
#include <ros/ros.h>
// moveit_grasps
#include "../include/grasp_candidate.h"
#include "../include/grasp_generator.h"
// moveit
#include <moveit_visual_tools/moveit_visual_tools.h>
namespace moveit_grasps
{
// Allow an interrupt to be called that waits for user input, useful for debugging
typedef boost::function<void(std::string message)> WaitForNextStepCallback;
class GraspPlanner
{
public:
/**
* \brief Constructor
*/
GraspPlanner(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools);
/**
* \brief Plan entire cartesian manipulation sequence
* \param grasp_candidates - GraspCandidates for which we will compute apprach, lift and retreat paths
* \param robot_state - robot_state to be used for computeCartesianPath
* \param planning_scene_monitor - Current state of the world
* \param grasp_data - robot gripper configuration
* \param grasp_object_id - The name of the target grasp object to be used in the planning scene.
* \return true on success
*/
bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates,
const robot_state::RobotStatePtr& robot_state,
const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
const std::string& grasp_object_id = "");
bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates,
const robot_state::RobotStatePtr& robot_state,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const std::string& grasp_object_id = "");
/**
* \brief Plan entire cartesian manipulation sequence
* \param input - description
* \return true on success
*/
bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state,
const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
bool verbose_cartesian_filtering, const std::string& grasp_object_id = "");
bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state,
const planning_scene::PlanningSceneConstPtr& planning_scene,
bool verbose_cartesian_filtering, const std::string& grasp_object_id = "");
/**
* \brief Compute a cartesian path along waypoints
* \return true on success
*/
bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate,
const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
const moveit::core::RobotStatePtr& start_state,
const EigenSTL::vector_Isometry3d& waypoints,
const std::string& grasp_object_id = "");
bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit::core::RobotStatePtr& start_state,
const EigenSTL::vector_Isometry3d& waypoints,
const std::string& grasp_object_id = "");
/**
* \brief Wait for user input to proceeed
* \param message - text to display to user when waiting
*/
void waitForNextStep(const std::string& message);
/**
* \brief Allow an interrupt to be called that waits for user input, useful for debugging
* \param message - text to display to user when waiting
*/
// TODO(davetcoleman): Ideally all the classes in moveit_grasps would leverage this approach
void setWaitForNextStepCallback(WaitForNextStepCallback callback);
/**
* \brief Load parameter settings of the server under ~/debug_level
* \param parent_name - only used for debugging, allows one to see what paremeters are loaded in what namespace
* \param setting_namespace - where on this node's namespace to load settings
* e.g. /this_name/setting_namespace/some_parameter
* \return true on success
*/
bool loadEnabledSettings();
/**
* \brief Check if a setting is enabled
* \param setting_name - name of key on the parameter server as loaded in the 'setting_namespace'
* \return true if setting is enabled
*/
bool isEnabled(const std::string& setting_name);
private:
// A shared node handle
ros::NodeHandle nh_;
// Class for publishing stuff to rviz
moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;
WaitForNextStepCallback wait_for_next_step_callback_;
// Visualization settings
bool enabled_settings_loaded_ = false;
std::map<std::string, bool> enabled_setting_;
}; // end class
// Create smart pointers for this class
typedef std::shared_ptr<GraspPlanner> GraspPlannerPtr;
typedef std::shared_ptr<const GraspPlanner> GraspPlannerConstPtr;
} // namespace moveit_grasps
#endif
src/mtc/include/grasp_planner.h deleted 100644 → 0
+ 0
− 163
View file @ 70d556b1
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, University of Colorado, Boulder
* 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 the Univ of CO, Boulder 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.
*********************************************************************/
/* Author: Dave Coleman <dave@picknik.ai>
Desc: Find the approach, lift, and retreat path for a candidate grasp (if a valid one exists)
*/
#ifndef MOVEIT_GRASPS__GRASP_PLANNER_
#define MOVEIT_GRASPS__GRASP_PLANNER_
// ROS
#include <ros/ros.h>
// moveit_grasps
#include "grasp_candidate.h"
#include "grasp_generator.h"
// moveit
#include <moveit_visual_tools/moveit_visual_tools.h>
namespace moveit_grasps
{
// Allow an interrupt to be called that waits for user input, useful for debugging
typedef boost::function<void(std::string message)> WaitForNextStepCallback;
class GraspPlanner
{
public:
/**
* \brief Constructor
*/
GraspPlanner(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools);
/**
* \brief Plan entire cartesian manipulation sequence
* \param grasp_candidates - GraspCandidates for which we will compute apprach, lift and retreat paths
* \param robot_state - robot_state to be used for computeCartesianPath
* \param planning_scene_monitor - Current state of the world
* \param grasp_data - robot gripper configuration
* \param grasp_object_id - The name of the target grasp object to be used in the planning scene.
* \return true on success
*/
bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates,
const robot_state::RobotStatePtr& robot_state,
const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
const std::string& grasp_object_id = "");
bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates,
const robot_state::RobotStatePtr& robot_state,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const std::string& grasp_object_id = "");
/**
* \brief Plan entire cartesian manipulation sequence
* \param input - description
* \return true on success
*/
bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state,
const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
bool verbose_cartesian_filtering, const std::string& grasp_object_id = "");
bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state,
const planning_scene::PlanningSceneConstPtr& planning_scene,
bool verbose_cartesian_filtering, const std::string& grasp_object_id = "");
/**
* \brief Compute a cartesian path along waypoints
* \return true on success
*/
bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate,
const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
const moveit::core::RobotStatePtr& start_state,
const EigenSTL::vector_Isometry3d& waypoints,
const std::string& grasp_object_id = "");
bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate,
const planning_scene::PlanningSceneConstPtr& planning_scene,
const moveit::core::RobotStatePtr& start_state,
const EigenSTL::vector_Isometry3d& waypoints,
const std::string& grasp_object_id = "");
/**
* \brief Wait for user input to proceeed
* \param message - text to display to user when waiting
*/
void waitForNextStep(const std::string& message);
/**
* \brief Allow an interrupt to be called that waits for user input, useful for debugging
* \param message - text to display to user when waiting
*/
// TODO(davetcoleman): Ideally all the classes in moveit_grasps would leverage this approach
void setWaitForNextStepCallback(WaitForNextStepCallback callback);
/**
* \brief Load parameter settings of the server under ~/debug_level
* \param parent_name - only used for debugging, allows one to see what paremeters are loaded in what namespace
* \param setting_namespace - where on this node's namespace to load settings
* e.g. /this_name/setting_namespace/some_parameter
* \return true on success
*/
bool loadEnabledSettings();
/**
* \brief Check if a setting is enabled
* \param setting_name - name of key on the parameter server as loaded in the 'setting_namespace'
* \return true if setting is enabled
*/
bool isEnabled(const std::string& setting_name);
private:
// A shared node handle
ros::NodeHandle nh_;
// Class for publishing stuff to rviz
moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;
WaitForNextStepCallback wait_for_next_step_callback_;
// Visualization settings
bool enabled_settings_loaded_ = false;
std::map<std::string, bool> enabled_setting_;
}; // end class
// Create smart pointers for this class
typedef std::shared_ptr<GraspPlanner> GraspPlannerPtr;
typedef std::shared_ptr<const GraspPlanner> GraspPlannerConstPtr;
} // namespace moveit_grasps
#endif
src/mtc/include/grasp_scorer.h deleted 100644 → 0
+ 0
− 126
View file @ 70d556b1
/*********************************************************************
* Software License Agreement ("Modified BSD License")
*
* Copyright (c) 2014, University of Colorado, Boulder
* 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 the Univ of CO, Boulder 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 : Andy McEvoy
* Desc : Functions for scoring generated grasps
*/
#ifndef MOVEIT_GRASPS__GRASP_SCORER_
#define MOVEIT_GRASPS__GRASP_SCORER_
#include <cmath>
#include <ros/ros.h>
#include "../include/grasp_data.h"
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace moveit_grasps
{
struct GraspScoreWeights
{
GraspScoreWeights()
: orientation_x_score_weight_(1.0)
, orientation_y_score_weight_(1.0)
, orientation_z_score_weight_(1.0)
, translation_x_score_weight_(1.0)
, translation_y_score_weight_(1.0)
, translation_z_score_weight_(1.0)
{
}
/* \brief Compute the weighted score given the orientation and translation scores */
double computeScore(const Eigen::Vector3d& orientation_scores, const Eigen::Vector3d& translation_scores,
bool verbose = false) const;
/* \brief returns the sum of the grasp score weights*/
virtual double getWeightTotal() const;
double orientation_x_score_weight_;
double orientation_y_score_weight_;
double orientation_z_score_weight_;
double translation_x_score_weight_;
double translation_y_score_weight_;
double translation_z_score_weight_;
};
// Create smart pointers for this struct
typedef std::shared_ptr<GraspScoreWeights> GraspScoreWeightsPtr;
class GraspScorer
{
public:
/**
* \brief Scores each axis of the grasp based on its angle to the desired pose axis.
* \param grasp_pose_tcp - the pose of the end effector
* \param ideal_pose - the ideal grasp pose (ex: straight into the bin)
* \return the unweighted scores:
* 1.0 -> 0 degrees between grasp axis and desired axis,
* 0.0 -> 180 degrees
*/
static Eigen::Vector3d scoreRotationsFromDesired(const Eigen::Isometry3d& grasp_pose_tcp,
const Eigen::Isometry3d& ideal_pose);
/**
* \brief Score the grasp based on the translation values of the grasp pose
* \param grasp_pose_tcp - the pose of the end effector (not the eef mount)
* \param min_translations - the minimum translation values for all grasp poses
* \param max_translations - the maximum translation values for all grasp poses
* \return the unweighted scores:
* 0.0 -> pose is at the minimum translation in that axis
* 1.0 -> pose is at the maximum translation in that axis
*/
static Eigen::Vector3d scoreGraspTranslation(const Eigen::Isometry3d& grasp_pose_tcp,
const Eigen::Vector3d& min_translations,
const Eigen::Vector3d& max_translations);
/**
* \brief Score the grasp based on the translation values of the grasp pose
* \param grasp_pose_tcp - the pose of the end effector (not the eef mount)
* \param ideal_pose - the ideal pose location
* \param object_pose - the pose of the object being grasped
* \param object_size - the size of the object represented as a vector [x,y,z]
* \param visual_tools - set to a moveit_visual_tools pointer to enable visual debugging
* \return the unweighted scores:
* 0.0 -> pose is at the ideal translation in that axis
*/
static Eigen::Vector3d scoreGraspTranslation(const Eigen::Isometry3d& grasp_pose_tcp,
const Eigen::Isometry3d& ideal_pose);
};
} // namespace moveit_grasps
#endif
src/mtc/include/impl/moveit_mediator.h
+ 39
− 6
View file @ fed861f9
......@@ -15,20 +15,49 @@
#include "impl/robot.h"
#include "impl/moveit_robot.h"
#include "impl/wing_moveit_decorator.h"
#include "impl/collision_helper.h"
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/stages/current_state.h>
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
#include <moveit/task_constructor/stages/generate_pose.h>
#include <moveit/task_constructor/stages/generate_place_pose.h>
#include <moveit/task_constructor/stages/modify_planning_scene.h>
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/predicate_filter.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
#include <eigen_conversions/eigen_msg.h>
class Moveit_mediator : public Abstract_mediator{
protected:
ros::NodeHandle nh_;
ros::Publisher collision_object_publisher_;
ros::Publisher planning_scene_diff_publisher_;
robot_model_loader::RobotModelLoader* robot_model_loader_;
robot_model::RobotModelPtr kinematic_model_;
planning_scene::PlanningScene* ps_;
XmlRpc::XmlRpcValue properties_;
std::shared_ptr<moveit::task_constructor::solvers::PipelinePlanner> sampling_planner_;
std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_;
std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_;
std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_;
public:
Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, ros::NodeHandle nh) : Abstract_mediator(objects, pub), nh_(nh){
collision_object_publisher_ = nh.advertise<moveit_msgs::CollisionObject>("collision_object", 1);
planning_scene_diff_publisher_ = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, ros::NodeHandle nh) : Abstract_mediator(objects, pub), nh_(nh), sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>()), cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>()), psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>()){
// planner
sampling_planner_->setProperty("goal_joint_tolerance", 1e-5);
// cartesian
cartesian_planner_->setMaxVelocityScaling(1.0);
cartesian_planner_->setMaxAccelerationScaling(1.0);
cartesian_planner_->setStepSize(.01);
};
void setup_task();
......@@ -39,9 +68,13 @@ class Moveit_mediator : public Abstract_mediator{
void set_wings(std::vector<std::vector<wing_BP>>& wbp) override;
void publish_tables(moveit::planning_interface::PlanningSceneInterface& psi);
void publish_tables();
void load_robot_description();
void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
void create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;};
};
#endif
\ No newline at end of file
src/mtc/include/impl/moveit_robot.h
+ 4
− 6
View file @ fed861f9
......@@ -17,16 +17,14 @@ class Moveit_robot : public Robot{
Moveit_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Robot(name, tf, size){
mgi_ = new moveit::planning_interface::MoveGroupInterface(name);
std::stringstream hand_n, ik_frame_n, name_n;
name_n << name;
hand_n << "hand_" << name.back();
ik_frame_n << "panda_" << name.back() << "_link8";
map_.insert(std::make_pair<std::string, std::string>("group", name_n.str()));
map_.insert(std::make_pair<std::string, std::string>("eef", hand_n.str()));
map_.insert(std::make_pair<std::string, std::string>("hand_grasping_frame", ik_frame_n.str()));
map_.insert(std::make_pair<std::string, std::string>("ik_frame", ik_frame_n.str()));
map_.insert(std::make_pair<std::string, std::string>("hand", hand_n.str()));
map_.insert(std::make_pair<std::string, std::string>("eef_name", hand_n.str()));
map_.insert(std::make_pair<std::string, std::string>("hand_frame", ik_frame_n.str()));
map_.insert(std::make_pair<std::string, std::string>("hand_group_name", hand_n.str()));
}
inline moveit::planning_interface::MoveGroupInterface* mgi() {return mgi_;}
......
src/mtc/include/state_validity_callback.h deleted 100644 → 0
+ 0
− 87
View file @ 70d556b1
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, University of Colorado, Boulder
* 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 the Univ of CO, Boulder 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.
*********************************************************************/
/* Author: Dave Coleman <dave@picknik.ai>
Desc: Callback for checking if a state is in collision
*/
#ifndef MOVEIT_GRASPS__STATE_VALIDITY_CALLBACK
#define MOVEIT_GRASPS__STATE_VALIDITY_CALLBACK
// Rviz
#include <moveit_visual_tools/moveit_visual_tools.h>
// MoveIt
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/robot_state/robot_state.h>
namespace
{
bool isGraspStateValid(const planning_scene::PlanningScene* planning_scene, bool visual_debug, double verbose_speed,
const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools,
robot_state::RobotState* robot_state, const robot_state::JointModelGroup* group,
const double* ik_solution)
{
robot_state->setJointGroupPositions(group, ik_solution);
robot_state->update();
if (!robot_state->satisfiesBounds(group))
{
ROS_DEBUG_STREAM_NAMED("is_grasp_state_valid", "Ik solution invalid");
return false;
}
if (!planning_scene)
{
ROS_ERROR_STREAM_NAMED("is_grasp_state_valid", "No planning scene provided");
return false;
}
if (!planning_scene->isStateColliding(*robot_state, group->getName()))
return true; // not in collision
// Display more info about the collision
if (visual_debug && visual_tools)
{
visual_tools->publishRobotState(*robot_state, rviz_visual_tools::RED);
planning_scene->isStateColliding(*robot_state, group->getName(), true);
visual_tools->publishContactPoints(*robot_state, planning_scene);
visual_tools->trigger();
ros::Duration(verbose_speed).sleep();
}
return false;
}
} // namespace
#endif
src/mtc/include/suction_voxel_matrix.h deleted 100644 → 0
+ 0
− 188
View file @ 70d556b1
/*
* Software License Agreement (Modified BSD License)
*
* Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L.
* 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 Univ of CO, Boulder, PAL Robotics, S.L.
* 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: Mike Lautman
Description: Data class used by grasp data.
*/
#ifndef MOVEIT_GRASPS__SUCTION_VOXEL_MATRIX_H_
#define MOVEIT_GRASPS__SUCTION_VOXEL_MATRIX_H_
// ROS
#include <ros/ros.h>
// Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace moveit_grasps
{
struct SuctionVoxel
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* \brief
@param center_point - position of voxel center point in tcp frame
@param x_width - width of voxel along x dim in tcp frame
@param y_width - width of voxel along y dim in tcp frame
*/
SuctionVoxel(const Eigen::Vector3d& center_point, double x_width, double y_width)
: center_point_(center_point), x_width_(x_width), y_width_(y_width)
{
top_left_ = center_point + Eigen::Vector3d(-x_width / 2.0, y_width / 2.0, 0);
top_right_ = center_point + Eigen::Vector3d(x_width / 2.0, y_width / 2.0, 0);
bottom_left_ = center_point + Eigen::Vector3d(-x_width / 2.0, -y_width / 2.0, 0);
bottom_right_ = center_point + Eigen::Vector3d(x_width / 2.0, -y_width / 2.0, 0);
}
// Voxel center point in tcp frame
Eigen::Vector3d center_point_;
double x_width_;
double y_width_;
// Voxel corners in tcp frame
Eigen::Vector3d top_left_;
Eigen::Vector3d top_right_;
Eigen::Vector3d bottom_left_;
Eigen::Vector3d bottom_right_;
};
class SuctionVoxelMatrix
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
SuctionVoxelMatrix(double suction_rows_count, double suction_cols_count, double total_suction_range_y,
double total_suction_range_x)
: suction_rows_count_(suction_rows_count)
, suction_cols_count_(suction_cols_count)
, active_suction_range_x_(total_suction_range_x)
, active_suction_range_y_(total_suction_range_y)
{
voxel_x_width_ = active_suction_range_x_ / suction_cols_count_;
voxel_y_width_ = active_suction_range_y_ / suction_rows_count_;
suction_voxels_.resize(suction_rows_count_);
// We store the voxels starting bottom left and moving right then up
for (std::size_t voxel_y = 0; voxel_y < suction_rows_count_; ++voxel_y)
{
suction_voxels_[voxel_y].resize(suction_cols_count_);
for (std::size_t voxel_x = 0; voxel_x < suction_cols_count_; ++voxel_x)
{
suction_voxels_[voxel_y][voxel_x] = std::make_shared<SuctionVoxel>(
Eigen::Vector3d(-active_suction_range_x_ / 2.0 + voxel_x_width_ * (voxel_x + 0.5),
-active_suction_range_y_ / 2.0 + voxel_y_width_ * (voxel_y + 0.5), 0),
voxel_x_width_, voxel_y_width_);
}
}
}
// \brief - get the voxel at the index location [row, col] with [0, 0] being the bottom left
bool getSuctionVoxel(std::size_t row, std::size_t col, std::shared_ptr<SuctionVoxel>& voxel)
{
if (row >= suction_rows_count_)
{
ROS_DEBUG_STREAM_NAMED("suction_voxel_matrix", "Invalid row " << row << "/" << suction_rows_count_ - 1);
return false;
}
if (col >= suction_cols_count_)
{
ROS_DEBUG_STREAM_NAMED("suction_voxel_matrix", "Invalid col " << col << "/" << suction_cols_count_ - 1);
return false;
}
voxel = suction_voxels_[row][col];
return true;
}
/** \brief Get the voxel at the index i where columns are the minor axis and rows are the major axes.
* @param index - the index of the suction voxel where index / #cols is the row and index % #cols is the col
* index 0 is bottom left
*/
bool getSuctionVoxel(std::size_t index, std::shared_ptr<SuctionVoxel>& voxel)
{
return getSuctionVoxel(index / suction_cols_count_, index % suction_cols_count_, voxel);
}
std::size_t getNumRows()
{
return suction_rows_count_;
}
std::size_t getNumCols()
{
return suction_cols_count_;
}
std::size_t getNumVoxels()
{
return suction_cols_count_ * suction_rows_count_;
}
double getVoxelArea()
{
return voxel_x_width_ * voxel_y_width_;
}
double getVoxelWidthX()
{
return voxel_x_width_;
}
double getVoxelWidthY()
{
return voxel_y_width_;
}
double getActiveSuctionWidthX()
{
return active_suction_range_x_;
}
double getActiveSuctionWidthY()
{
return active_suction_range_y_;
}
protected:
std::size_t suction_rows_count_;
std::size_t suction_cols_count_;
double voxel_x_width_;
double voxel_y_width_;
double active_suction_range_x_;
double active_suction_range_y_;
std::vector<std::vector<std::shared_ptr<SuctionVoxel>>> suction_voxels_;
};
} // namespace moveit_grasps
#endif
src/mtc/include/two_finger_grasp_data.h deleted 100644 → 0
+ 0
− 120
View file @ 70d556b1
/*
* Software License Agreement (Modified BSD License)
*
* Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L.
* 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 Univ of CO, Boulder, PAL Robotics, S.L.
* 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: Dave Coleman, Bence Magyar
Description: Data class used by the grasp generator.
*/
#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_DATA_H_
#define MOVEIT_GRASPS__TWO_FINGER_GRASP_DATA_H_
// moveit grasps
#include "../include/grasp_data.h"
namespace moveit_grasps
{
MOVEIT_CLASS_FORWARD(TwoFingerGraspData);
class TwoFingerGraspData : public GraspData
{
public:
/**
* \brief Creates a two finger grasp data object
* \param node_handle - allows for namespacing
* \param end_effector name - which side of a two handed robot to load data for. should correspond to SRDF EE names
* \param robot_model - The robot model
*/
TwoFingerGraspData(const ros::NodeHandle& nh, const std::string& end_effector,
const moveit::core::RobotModelConstPtr& robot_model);
/**
* \brief Helper function for constructor, loads grasp data from a yaml file (load from roslaunch)
* \param nh - node handle allows for namespacing
* \param end_effector - The end effector joint group name
* \return true on success
*/
bool loadGraspData(const ros::NodeHandle& nh, const std::string& end_effector) override;
/**
* \brief Set the width between fingers as a percentage of object size and max finger width
* \param fraction_open - [0,1] the 0->closed 1->open
* \return true on success
*/
bool setGraspWidth(double fraction_open, double min_finger_width, trajectory_msgs::JointTrajectory& grasp_posture);
/**
* \brief Convert width between fingers to joint positions
* \param distance_btw_fingers - (meters) The target distance between the fingers
* \param grasp_posture - output. A joint trajectory with the values for the end effector filled in.
* Note: we interpolate to get the joint value assuming a linear relationship between min and max
* finger distance and min and max finger joint value. This could likely be improved on a per-robot basis.
* \param end_effector - The end effector joint group name
* \return true on success
*/
bool fingerWidthToGraspPosture(double distance_btw_fingers, trajectory_msgs::JointTrajectory& grasp_posture);
/**
* \brief Convert joint positions to full grasp posture
* \param joint_positions - the full joint state as a vector of doubles
* \param grasp_posture - output. The full grasp posture
* \return true on success
*/
bool jointPositionsToGraspPosture(const std::vector<double>& joint_positions,
trajectory_msgs::JointTrajectory& grasp_posture);
/**
* \brief Debug data to console
*/
void print() override;
public:
/////////////////////////////////////
// Finger gripper specific parameters
/////////////////////////////////////
// Maximum allowed finger width for a grasp.
// This value should be considerably smaller than max_finger_width
// to allow padded collision checks
double max_grasp_width_;
// Maximum / Minimum distance between fingers
// For calculating the ratio between the distance between fingers and the joint values
double max_finger_width_;
double min_finger_width_;
// Parameter used to ensure generated grasps will overlap object
double gripper_finger_width_;
};
} // namespace moveit_grasps
#endif
src/mtc/include/two_finger_grasp_filter.h deleted 100644 → 0
+ 0
− 81
View file @ 70d556b1
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, University of Colorado, Boulder
* 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 the Univ of CO, Boulder 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.
*********************************************************************/
/* Author: Dave Coleman <dave@picknik.ai>
Desc: Filters grasps based on kinematic feasibility and collision
*/
#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_FILTER_
#define MOVEIT_GRASPS__TWO_FINGER_GRASP_FILTER_
// Parent class
#include "../include/grasp_filter.h"
// Grasping
#include "../include/two_finger_grasp_generator.h"
namespace moveit_grasps
{
class TwoFingerGraspFilter : public GraspFilter
{
public:
// Constructor
TwoFingerGraspFilter(const robot_state::RobotStatePtr& robot_state,
const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools);
/**
* \brief Thread for checking part of the possible grasps list
*/
bool processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct) override;
/**
* \brief Check if ik solution is in collision with fingers closed
* \return true on success
*/
bool checkFingersClosedIK(std::vector<double>& ik_solution, const IkThreadStructPtr& ik_thread_struct,
GraspCandidatePtr& grasp_candidate,
const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const;
protected:
// Name for logging
const std::string name_;
}; // end of class
typedef std::shared_ptr<TwoFingerGraspFilter> TwoFingerGraspFilterPtr;
typedef std::shared_ptr<const TwoFingerGraspFilter> TwoFingerGraspFilterConstPtr;
} // namespace moveit_grasps
#endif
src/mtc/include/two_finger_grasp_generator.h deleted 100644 → 0
+ 0
− 257
View file @ 70d556b1
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, University of Colorado, Boulder
* 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 the Univ of CO, Boulder 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.
*********************************************************************/
/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy
Desc: Generates geometric grasps for cuboids and blocks, not using physics or contact wrenches
*/
#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_GENERATOR_H_
#define MOVEIT_GRASPS__TWO_FINGER_GRASP_GENERATOR_H_
#include "../include/grasp_generator.h"
#include "../include/grasp_candidate.h"
#include "../include/two_finger_grasp_scorer.h"
#include "../include/two_finger_grasp_data.h"
// Testing
#include <gtest/gtest.h>
namespace moveit_grasps
{
struct TwoFingerGraspCandidateConfig
{
TwoFingerGraspCandidateConfig();
void enableAllGraspTypes();
void enableAllGraspAxes();
void enableAll();
void disableAllGraspTypes();
void disableAllGraspAxes();
void disableAll();
///////////////////////////////
// Finger Gripper config values
///////////////////////////////
bool enable_corner_grasps_;
bool enable_face_grasps_;
bool enable_variable_angle_grasps_;
bool enable_edge_grasps_;
bool generate_x_axis_grasps_;
bool generate_y_axis_grasps_;
bool generate_z_axis_grasps_;
};
class TwoFingerGraspGenerator : public GraspGenerator
{
public:
// Eigen requires 128-bit alignment for the Eigen::Vector2d's array (of 2 doubles).
// With GCC, this is done with a attribute ((aligned(16))).
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* \brief Constructor
*/
TwoFingerGraspGenerator(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, bool verbose = false);
/* brief sets internal grasp_candidate_config_ variable
* \param grasp_candidate_config - a config describing the grasps to be generated
*/
void setGraspCandidateConfig(const TwoFingerGraspCandidateConfig& grasp_candidate_config);
/**
* \brief Create possible grasp positions around a cuboid
* \param cuboid_pose - centroid of object to grasp in world frame
* \param depth length of cuboid along local x-axis
* \param width length of cuboid along local y-axis
* \param height length of cuboid along local z-axis
* \param grasp_data data describing end effector
* \param grasp_candidate_config parameter for selectively enabling and disabling different grasp types
* \param grasp_candidates possible grasps generated
* \return true if successful
*/
bool generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height,
const GraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates) override;
bool generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height,
const TwoFingerGraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates);
/**
* \brief Setter for grasp score weights
*/
void setGraspScoreWeights(const TwoFingerGraspScoreWeightsPtr& grasp_score_weights)
{
auto two_finger_grasp_score_weights = std::make_shared<TwoFingerGraspScoreWeights>(*grasp_score_weights);
grasp_score_weights_ = std::dynamic_pointer_cast<GraspScoreWeights>(two_finger_grasp_score_weights);
}
/**
* \brief Setter for grasp score weights
*/
const TwoFingerGraspScoreWeightsPtr getGraspScoreWeights()
{
return std::dynamic_pointer_cast<TwoFingerGraspScoreWeights>(grasp_score_weights_);
}
protected:
/**
* \brief creates grasp messages from the generated grasp poses
* \param grasp_pose_eef_mount - the grasp pose. (Note: this is the pose of the eef mount not the position of the tcp)
* \param grasp_data data describing the end effector
* \param object_pose - pose of object to grasp
* \param object_size - size of object to grasp
* \param object_width - In the case of finger grippers, the width of the object in the dimension betwen the fingers
* \param grasp_candidates - list possible grasps with new grasp appended
* \return true on success
*/
bool addGrasp(const Eigen::Isometry3d& grasp_pose_eef_mount, const TwoFingerGraspDataPtr& grasp_data,
const Eigen::Isometry3d& object_pose, const Eigen::Vector3d& object_size, double object_width,
std::vector<GraspCandidatePtr>& grasp_candidates);
/**
* \brief Create grasp positions around one axis of a cuboid
* \param cuboid_pose: centroid of object to grasp in world frame
* \param depth: length of cuboid along local x-axis
* \param width: length of cuboid along local y-axis
* \param height: length of cuboid along local z-axis
* \param axis: axis of cuboid to generate grasps along
* \param grasp_data: data describing end effector
* \param grasp_candidates: possible grasps generated
* \param only_edge_grasps: set to true if object is too wide to grap the face in this axis
* \return true if successful
*/
bool generateCuboidAxisGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height,
grasp_axis_t axis, const TwoFingerGraspDataPtr& grasp_data,
const TwoFingerGraspCandidateConfig& grasp_candidate_config,
std::vector<GraspCandidatePtr>& grasp_candidates);
/**
* \brief helper function for adding grasps at corner of cuboid
* \param pose - pose of the object to grasp
* \param rotation_angles - rotation angles to go from cuboid pose to standard grasping pose
* \param translation - translation to go from cuboid centroid to grasping location
* \param corner_rotation - extra rotatation needed to align grasp pose as you move around the cuboid
* \param num_radial_grasps - the number of grasps to generate around the corner
* \param grasp_poses_tcp - list of grasp poses generated
* \return the number of poses generated
*/
std::size_t addCornerGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3],
const Eigen::Vector3d& translation, double corner_rotation,
std::size_t num_radial_grasps, EigenSTL::vector_Isometry3d& grasp_poses_tcp);
/**
* \brief helper function for adding grasps along the face of a cuboid
* \param pose - pose of the object to grasp
* \param rotation_angles - rotation angles to go from cuboid pose to standard grasping pose
* \param delta - distance to move away from cuboid at each step
* \param translation - translation to go from cuboid centroid to grasping location
* \param alignment_rotation - extra rotatation needed to align grasp pose as you move around the cuboid
* \param num_grasps - the number of grasps to generate around the corner
* \param grasp_poses_tcp - list of grasp poses generated
* \return the number of poses generated
*/
std::size_t addFaceGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3],
const Eigen::Vector3d& translation, const Eigen::Vector3d& delta,
double alignment_rotation, std::size_t num_grasps,
EigenSTL::vector_Isometry3d& grasp_poses_tcp);
/**
* \brief helper function for adding grasps along the edges of the cuboid
* \param pose - pose of the object to grasp
* \param rotation_angles - rotation angles to go from cuboid pose to standard grasping pose
* \param delta - distance to move away from cuboid at each step
* \param translation - translation to go from cuboid centroid to grasping location
* \param alignment_rotation - extra rotatation needed to align grasp pose as you move around the cuboid
* \param num_grasps - the number of grasps to generate around the corner
* \param grasp_poses_tcp - list of grasp poses generated
* \return the number of poses generated
*/
std::size_t addEdgeGraspsHelper(const Eigen::Isometry3d& cuboid_pose, double rotation_angles[3],
const Eigen::Vector3d& translation, const Eigen::Vector3d& delta,
double alignment_rotation, std::size_t num_grasps,
EigenSTL::vector_Isometry3d& grasp_poses_tcp, double corner_rotation);
/**
* \brief helper function for determining if the grasp will intersect the cuboid
* \param cuboid_pose - centroid of object to grasp in world frame
* \param depth - size of cuboid along x axis
* \param width - size of cuboid along y axis
* \param height - size of cuboid along z axis
* \param grasp_pose_tcp - pose of grasp
* \param grasp_data - data describing end effector
* \return true if the grasp intersects the cuboid
*/
bool graspIntersectionHelper(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height,
const Eigen::Isometry3d& grasp_pose_tcp, const TwoFingerGraspDataPtr& grasp_data);
/**
* \brief helper function to test intersection of a line with a plane
* \param t - parametric distance along grasp line
* \param u1, v1, u2, v2 - (u,v) coordinates of the line
* \param a, b - length and width of the plane area in which to test for intersection
* \param u, v - location of intersection
* \return true if the line intersects the plane
*/
bool intersectionHelper(double t, double u1, double v1, double u2, double v2, double a, double b, double& u,
double& v);
/**
* \brief Score the generated finger grasp poses
* \param grasp_pose_tcp - the grasp pose of the tcp
* \param grasp_data - data describing the end effector
* \param object_pose - the pose of the object being grasped
* \param percent_open - percentage that the grippers are open. 0.0 -> grippers are at object width + padding
* \return a score with positive being better
*/
double scoreFingerGrasp(const Eigen::Isometry3d& grasp_pose_tcp, const TwoFingerGraspDataPtr& grasp_data,
const Eigen::Isometry3d& object_pose, double percent_open);
bool
generateFingerGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height,
const TwoFingerGraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates,
const TwoFingerGraspCandidateConfig& grasp_candidate_config = TwoFingerGraspCandidateConfig());
protected:
TwoFingerGraspCandidateConfig grasp_candidate_config_;
// Tests
FRIEND_TEST(TwoFingerGraspGeneratorTest, GenerateFaceGrasps);
FRIEND_TEST(TwoFingerGraspGeneratorTest, GenerateEdgeGrasps);
FRIEND_TEST(TwoFingerGraspGeneratorTest, GenerateCornerGrasps);
}; // end of class
typedef std::shared_ptr<TwoFingerGraspGenerator> TwoFingerGraspGeneratorPtr;
typedef std::shared_ptr<const TwoFingerGraspGenerator> TwoFingerGraspGeneratorConstPtr;
} // namespace moveit_grasps
#endif
src/mtc/include/two_finger_grasp_scorer.h deleted 100644 → 0
+ 0
− 104
View file @ 70d556b1
/*********************************************************************
* Software License Agreement ("Modified BSD License")
*
* Copyright (c) 2014, University of Colorado, Boulder
* 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 the Univ of CO, Boulder 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 : Andy McEvoy
* Desc : Functions for scoring generated grasps
*/
#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_SCORER_
#define MOVEIT_GRASPS__TWO_FINGER_GRASP_SCORER_
#include "../include/two_finger_grasp_data.h"
#include "../include/grasp_scorer.h"
namespace moveit_grasps
{
struct TwoFingerGraspScoreWeights : public GraspScoreWeights
{
TwoFingerGraspScoreWeights() : GraspScoreWeights(), depth_score_weight_(1.0), width_score_weight_(1.0)
{
}
/* \brief Compute the weighted score given the orientation and translation scores */
double computeScore(const Eigen::Vector3d& orientation_scores, const Eigen::Vector3d& translation_scores,
double depth_score, double width_score, bool verbose = false) const;
/* \brief returns the sum of the grasp score weights*/
double getWeightTotal() const override;
// Finger gripper specific weights
double depth_score_weight_;
double width_score_weight_;
};
// Create smart pointers for this class
typedef std::shared_ptr<TwoFingerGraspScoreWeights> TwoFingerGraspScoreWeightsPtr;
class TwoFingerGraspScorer : public GraspScorer
{
public:
/**
* \brief Scores the grasp on how wide the fingers are on approach, the more open the better
* \param grasp_data - pointer to grasp info
* \param percent_open - amount the gripper is open
* 0.0 -> gripper is open to the object width + minimum padding
* 1.0 -> gripper is in full open position
* \return the unweighted score:
* 1.0 -> gripper is wide open,
* 0.0 -> gripper is at minimum position.
*/
static double scoreGraspWidth(const TwoFingerGraspDataPtr& grasp_data, double percent_open);
/**
* \brief Score the grasp based on how far the object is from the palm of the hand
* \param grasp_pose_tcpgrasp_pose_tcp - the pose of the end effector (not the eef mount)
* \param grasp_data - pointer to grasp info
* \param object_pose - the pose of the object being grasped
* \param max_grasp_distance - the maximum acceptable distance from palm
* \return the unweighted score:
* 1.0 -> object pose and grasp pose have same translation values
* 0.0 -> object is at max distanct
* < 0.0 -> object is beyond the max_grasp_distance
*/
// DEV NOTE: when this function is called we've lost the references to the acutal size of the object.
// max_distance should be the length of the fingers minus some minimum amount that the fingers need to grip an object
// since we don't know the distance from the centoid of the object to the edge of the object, this is set as an
// arbitrary number given our target object set (i.e. I based it off of the cheese it box)
static double scoreDistanceToPalm(const Eigen::Isometry3d& grasp_pose_tcp, const TwoFingerGraspDataPtr& grasp_data,
const Eigen::Isometry3d& object_pose, double min_grasp_distance,
double max_grasp_distance);
};
} // namespace moveit_grasps
#endif
src/mtc/launch/cell_routine.launch
+ 1
− 1
View file @ fed861f9
<launch>
<!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
<rosparam command="load" file="$(find mtc)/results/dummy.yaml"/>
<rosparam command="load" file="$(find mtc)/results/dummy/-151902566.yaml"/>
<rosparam command="load" file="$(find mtc)/mtc_templates/dummy.yaml" />
<rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/>
<rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/>
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment