diff --git a/src/grasping/environment_util.h b/src/grasping/environment_util.h index d01cf889137e91daa255033055f5f18843e101d5..01a6fe5fa06abb75e1f8018dfa131ae81be3e9b5 100644 --- a/src/grasping/environment_util.h +++ b/src/grasping/environment_util.h @@ -32,7 +32,7 @@ public: double y_dimension); /** - * Adds a support surfacce to the planning scene (with ID "bin). + * Adds a support surface to the planning scene (with ID "bin"). * @param collision_objects current collision objects in the planning scene * @param id id of the support surface to create * @param object_x_dimension x dimension object to be picked diff --git a/src/grasping/grasp_service.cpp b/src/grasping/grasp_service.cpp index fff35996bc65e1da9d5488f23ece32f5d744db29..bca8656e91b67a8821567598793dfa80e8c5b40e 100644 --- a/src/grasping/grasp_service.cpp +++ b/src/grasping/grasp_service.cpp @@ -29,12 +29,11 @@ tf2_ros::Buffer tfBuffer; /** * Move the robot to a pose between different pick/place-actions. * Solves currently the problem of the wrong eef-to-object orientation. - * @param planning_scene_interface a valid PlanningSceneInterface instance * @param group a initialized MoveGroupInterface instance * @return true on success */ -bool movetoInitPose(moveit::planning_interface::PlanningSceneInterface &planning_scene_interface, - moveit::planning_interface::MoveGroupInterface &group) { +bool movetoInitPose(moveit::planning_interface::MoveGroupInterface &group) { + geometry_msgs::Pose another_pose; another_pose.orientation.w = 0; another_pose.orientation.x = 0.981; @@ -100,7 +99,7 @@ bool pickObject(panda_grasping::PickObject::Request &req, } ROS_INFO("Moving to initial pose."); - if (!movetoInitPose(planning_scene_interface, group)) { + if (!movetoInitPose(group)) { res.success = false; return false; } diff --git a/src/grasping/grasp_util.h b/src/grasping/grasp_util.h index 54762416eafe194993392ac23131c585f4ca694c..e2eff0019dc286257cea719b93e3c6557f4347cd 100644 --- a/src/grasping/grasp_util.h +++ b/src/grasping/grasp_util.h @@ -50,7 +50,7 @@ public: /** * Util method to pick an object from above. - * @param move_group a initialized MoveGroupInterface instance + * @param move_group an initialized MoveGroupInterface instance * @param pick_pose via getPickFromAbovePose-method computed pre-pick-pose * @param close_amount total close amount (space between "forks") * @param open_amount total opening amount (space between "forks") @@ -64,7 +64,7 @@ public: /** * Util method to place an object from above. - * @param move_group a initialized MoveGroupInterface instance + * @param move_group an initialized MoveGroupInterface instance * @param place_pose target place pose for the object * @param close_amount total close amount (space between "forks") * @param open_amount total opening amount (space between "forks")