Skip to content
Snippets Groups Projects
Commit d898ba2e authored by Johannes Mey's avatar Johannes Mey
Browse files

align file names and remove old files

parent c3dfb598
No related branches found
No related tags found
No related merge requests found
...@@ -5,8 +5,8 @@ add_compile_options(-std=c++14) ...@@ -5,8 +5,8 @@ add_compile_options(-std=c++14)
set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive") set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}")
# Find catkin macros and libraries if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) is used, also find other # Find catkin macros and libraries if COMPONENTS list like `find_package(catkin REQUIRED COMPONENTS xyz)` is used,
# catkin packages # also find other catkin packages
find_package(catkin REQUIRED find_package(catkin REQUIRED
COMPONENTS controller_manager COMPONENTS controller_manager
effort_controllers effort_controllers
...@@ -30,10 +30,11 @@ find_package(catkin REQUIRED ...@@ -30,10 +30,11 @@ find_package(catkin REQUIRED
# System dependencies are found with CMake's conventions # System dependencies are found with CMake's conventions
find_package(PkgConfig REQUIRED) find_package(PkgConfig REQUIRED)
# ################################################################################################################################ # ######################################################################################################################
# catkin specific configuration ## # catkin specific configuration ##
# ################################################################################################################################ # ######################################################################################################################
# The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects # The catkin_package macro generates cmake config files for your package and declare things to be passed to dependent
# projects. This has to be done in addition to the find_package command above.
catkin_package(CATKIN_DEPENDS catkin_package(CATKIN_DEPENDS
moveit_core moveit_core
moveit_visual_tools moveit_visual_tools
...@@ -46,9 +47,9 @@ catkin_package(CATKIN_DEPENDS ...@@ -46,9 +47,9 @@ catkin_package(CATKIN_DEPENDS
# system_lib # system_lib
) )
# ################################################################################################################################ # ######################################################################################################################
# Build ## # Build ##
# ################################################################################################################################ # ######################################################################################################################
# Specify additional locations of header files Your package locations should be listed before other locations # Specify additional locations of header files Your package locations should be listed before other locations
include_directories(${catkin_INCLUDE_DIRS}) include_directories(${catkin_INCLUDE_DIRS})
...@@ -57,13 +58,11 @@ add_executable(BasicCartesianPlanner src/BasicCartesianPlanner.cpp) ...@@ -57,13 +58,11 @@ add_executable(BasicCartesianPlanner src/BasicCartesianPlanner.cpp)
add_executable(BasicJointSpacePlanner src/BasicJointSpacePlanner.cpp) add_executable(BasicJointSpacePlanner src/BasicJointSpacePlanner.cpp)
add_executable(ObstacleAwarePlanner src/ObstacleAwarePlanner.cpp) add_executable(ObstacleAwarePlanner src/ObstacleAwarePlanner.cpp)
add_executable(GraspPlanner src/GraspPlanner.cpp) add_executable(GraspPlanner src/GraspPlanner.cpp)
add_executable(${PROJECT_NAME}_pick_place src/pick_place_tutorial.cpp) add_executable(PickPlacePlanner src/PickPlacePlanner.cpp)
set_target_properties(${PROJECT_NAME}_pick_place PROPERTIES OUTPUT_NAME pick_place PREFIX "")
## Add cmake target dependencies of the executable ## Add cmake target dependencies of the executable
target_link_libraries(BasicCartesianPlanner ${catkin_LIBRARIES}) target_link_libraries(BasicCartesianPlanner ${catkin_LIBRARIES})
target_link_libraries(BasicJointSpacePlanner ${catkin_LIBRARIES}) target_link_libraries(BasicJointSpacePlanner ${catkin_LIBRARIES})
target_link_libraries(ObstacleAwarePlanner ${catkin_LIBRARIES}) target_link_libraries(ObstacleAwarePlanner ${catkin_LIBRARIES})
target_link_libraries(GraspPlanner ${catkin_LIBRARIES}) target_link_libraries(GraspPlanner ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME}_pick_place ${catkin_LIBRARIES}) target_link_libraries(PickPlacePlanner ${catkin_LIBRARIES})
...@@ -51,13 +51,14 @@ In this example, an object can be observed in the RViz planning window. This obj ...@@ -51,13 +51,14 @@ In this example, an object can be observed in the RViz planning window. This obj
- Expected results: - Expected results:
- A Gazebo window to observe the executed robot motion opens - A Gazebo window to observe the executed robot motion opens
- An rqt window to show log messages opens - An rqt window to show log messages opens
- An RViz window to see the plan and trigger the motion open. - An RViz window to show the plan and trigger the motion open
- The motion does **not** start automatically, instead it must be triggered by pressing *next* in the RvizVisualToolsGUI within RViz - The motion does **not** start automatically, instead it must be triggered by pressing *next* in the RvizVisualToolsGUI within RViz
### Execution of a Pick-and-Place Task ### Execution of a Pick-and-Place Task
- Command: `roslaunch sample_applications pick_place_simulation.launch` - Command: `roslaunch sample_applications pick_place_planner.launch`
- Expected results: - Expected results:
- An RViz window to show the pick and place plan
- A Gazebo window to observe the executed robot motion opens - A Gazebo window to observe the executed robot motion opens
- Two tables an a "stick" on top of one of them are spawned in Gazebo - Two tables an a "stick" on top of one of them are spawned in Gazebo
- The Robot positions itself behind the stick and grasps it from the side. - The Robot positions itself behind the stick and grasps it from the side.
...@@ -75,7 +76,7 @@ In this example, an object can be observed in the RViz planning window. This obj ...@@ -75,7 +76,7 @@ In this example, an object can be observed in the RViz planning window. This obj
This is an example to use RViz and Gazebo interactively. Motions can be planned and executed from the RViz GUI. This is an example to use RViz and Gazebo interactively. Motions can be planned and executed from the RViz GUI.
- Command: `roslaunch sample_applications simulation.launch` - Command: `roslaunch sample_applications interactive_planner.launch`
- Expected results: - Expected results:
- RViz window to control the robot - RViz window to control the robot
- Gazebo window to observe the executed robot motion - Gazebo window to observe the executed robot motion
......
<launch>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
<node name="spawn_object" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/object.urdf -model c_object" respawn="false" output="screen" />
<node name="spawn_table1" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/table1.urdf -model c_table1" respawn="false" output="screen" />
<node name="spawn_table2" pkg="gazebo_ros" type="spawn_model" args="-urdf -z 1 -file $(find tutorial_moveit_motion_planning)/models/table2.urdf -model c_table2" respawn="false" output="screen" />
<node pkg="sample_applications" type="GraspPlanner" name="GraspPlannerInstance" respawn="false" output="screen"/>
</launch>
File moved
...@@ -2,12 +2,8 @@ ...@@ -2,12 +2,8 @@
<include file="$(find panda_simulation)/launch/simulation.launch"/> <include file="$(find panda_simulation)/launch/simulation.launch"/>
<!-- <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />--> <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
<!-- <rosparam file="$(find sample_applications)/config/config.yaml" command="load" />-->
<!-- <node name="safety_zone_spawner" pkg="sample_applications" type="safety_zone_spawner" respawn="false" output="screen"/>-->
<node pkg="sample_applications" type="pick_place" name="pick_place" /> <node pkg="sample_applications" type="pick_place" name="pick_place" />
<!-- <node name="rqt_console" pkg="rqt_console" type="rqt_console" />-->
</launch> </launch>
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="simple_box">
<link name="object_base_link">
</link>
<joint name="object_base_joint" type="fixed">
<parent link="object_base_link"/>
<child link="object_link"/>
<axis xyz="0 0 1" />
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<link name="object_link">
<inertial>
<origin xyz="0.4 -0.2 1.0" />
<mass value="1.0" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />
</inertial>
<visual>
<origin xyz="0.4 -0.2 1.0"/>
<geometry>
<box size="0.4 0.1 0.4" />
</geometry>
</visual>
<collision>
<origin xyz="0.4 -0.2 1.0"/>
<geometry>
<box size="0.4 0.1 0.4" />
</geometry>
</collision>
</link>
<gazebo reference="object_base_link">
<gravity>0</gravity>
</gazebo>
<gazebo reference="object_link">
<material>Gazebo/Blue</material>
</gazebo>
</robot>
\ No newline at end of file
// ROS
#include <ros/ros.h>
// MoveIt!
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
// TF2
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
geometry_msgs::Quaternion q_pick_object;
geometry_msgs::Pose getTargetPickPose(){
std::vector<double> co_dim{0.02, 0.02, 0.2};
std::vector<double> co_pose{0.5, 0.0, 0.2};
geometry_msgs::Pose target_pose;
tf2::Quaternion orientation_gripper(-0.924, 0.383, 0, 0);
tf2::Quaternion orientation_object(q_pick_object.x, q_pick_object.y, q_pick_object.z, q_pick_object.w);
tf2::Quaternion orientation = orientation_gripper * orientation_object;
orientation.setZ(orientation.getZ() * -1);
orientation.setY(orientation.getY() * -1);
orientation.setX(orientation.getX() * -1);
target_pose.orientation = tf2::toMsg(orientation);
target_pose.position.x = co_pose[0];
target_pose.position.y = co_pose[1];
target_pose.position.z = co_pose[2] + 0.3;
return target_pose;
}
void openGripper(trajectory_msgs::JointTrajectory& posture)
{
// BEGIN_SUB_TUTORIAL open_gripper
/* Add both finger joints of panda robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2";
/* Set them as open, wide enough for the object to fit. */
posture.points.resize(1);
posture.points[0].positions.resize(2);
posture.points[0].positions[0] = 0.04;
posture.points[0].positions[1] = 0.04;
posture.points[0].time_from_start = ros::Duration(0.5);
// END_SUB_TUTORIAL
}
void closedGripper(trajectory_msgs::JointTrajectory& posture)
{
// BEGIN_SUB_TUTORIAL closed_gripper
/* Add both finger joints of panda robot. */
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
posture.joint_names[1] = "panda_finger_joint2";
/* Set them as closed. */
posture.points.resize(1);
posture.points[0].positions.resize(2);
posture.points[0].positions[0] = 0.0;
posture.points[0].positions[1] = 0.0;
std::cout << "pos: " << posture.points[0].positions[0] << std::endl;
posture.points[0].effort.resize(2);
posture.points[0].effort[0] = 10;
posture.points[0].effort[1] = 10;
std::cout << "s:" << posture.points[0].effort.size() << std::endl;
posture.points[0].time_from_start = ros::Duration(0.5);
// END_SUB_TUTORIAL
}
void pick(moveit::planning_interface::MoveGroupInterface& move_group)
{
// BEGIN_SUB_TUTORIAL pick1
// Create a vector of grasps to be attempted, currently only creating single grasp.
// This is essentially useful when using a grasp generator to generate and test multiple grasps.
std::vector<moveit_msgs::Grasp> grasps;
grasps.resize(1);
// Setting grasp pose
// ++++++++++++++++++++++
// This is the pose of panda_link8. |br|
// From panda_link8 to the palm of the eef the distance is 0.058, the cube starts 0.01 before 5.0 (half of the length
// of the cube). |br|
// Therefore, the position for panda_link8 = 5 - (length of cube/2 - distance b/w panda_link8 and palm of eef - some
// extra padding)
grasps[0].grasp_pose.header.frame_id = "panda_link0";
/*tf2::Quaternion orientation;
orientation.setRPY(-M_PI / 2, -M_PI / 4, -M_PI / 2);
grasps[0].grasp_pose.pose.orientation = tf2::toMsg(orientation);
grasps[0].grasp_pose.pose.position.x = 0.415;
grasps[0].grasp_pose.pose.position.y = 0;
grasps[0].grasp_pose.pose.position.z = 0.5;*/
grasps[0].grasp_pose.pose = getTargetPickPose();
// Setting pre-grasp approach
// ++++++++++++++++++++++++++
/* Defined with respect to frame_id */
grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0";
/* Direction is set as positive x axis */
grasps[0].pre_grasp_approach.direction.vector.z = -1.0;
grasps[0].pre_grasp_approach.min_distance = 0.095;
grasps[0].pre_grasp_approach.desired_distance = 0.115;
// Setting post-grasp retreat
// ++++++++++++++++++++++++++
/* Defined with respect to frame_id */
grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0";
/* Direction is set as positive z axis */
grasps[0].post_grasp_retreat.direction.vector.z = 1.0;
grasps[0].post_grasp_retreat.min_distance = 0.1;
grasps[0].post_grasp_retreat.desired_distance = 0.25;
grasps[0].max_contact_force = 12;
//std::vector<std::string> atos {"c_object", "panda_rightfinger", "panda_leftfinger"};
//grasps[0].allowed_touch_objects = atos;
// Setting posture of eef before grasp
// +++++++++++++++++++++++++++++++++++
openGripper(grasps[0].pre_grasp_posture);
// END_SUB_TUTORIAL
// BEGIN_SUB_TUTORIAL pick2
// Setting posture of eef during grasp
// +++++++++++++++++++++++++++++++++++
closedGripper(grasps[0].grasp_posture);
// END_SUB_TUTORIAL
// BEGIN_SUB_TUTORIAL pick3
// Set support surface as table1.
move_group.setSupportSurfaceName("c_table1");
// Call pick to pick up the object using the grasps given
move_group.setMaxVelocityScalingFactor(0.05);
move_group.setMaxAccelerationScalingFactor(0.05);
move_group.pick("c_object", grasps);
//move_group.attachObject("c_object");
// END_SUB_TUTORIAL
}
void place(moveit::planning_interface::MoveGroupInterface& group)
{
// BEGIN_SUB_TUTORIAL place
// TODO(@ridhwanluthra) - Calling place function may lead to "All supplied place locations failed. Retrying last
// location in
// verbose mode." This is a known issue and we are working on fixing it. |br|
// Create a vector of placings to be attempted, currently only creating single place location.
std::vector<moveit_msgs::PlaceLocation> place_location;
place_location.resize(1);
// Setting place location pose
// +++++++++++++++++++++++++++
place_location[0].place_pose.header.frame_id = "panda_link0";
tf2::Quaternion orientation;
orientation.setRPY(0, 0, M_PI / 2);
place_location[0].place_pose.pose.orientation = tf2::toMsg(orientation);
/* While placing it is the exact location of the center of the object. */
place_location[0].place_pose.pose.position.x = 0;
place_location[0].place_pose.pose.position.y = 0.5;
place_location[0].place_pose.pose.position.z = 0.3;
// Setting pre-place approach
// ++++++++++++++++++++++++++
/* Defined with respect to frame_id */
place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0";
/* Direction is set as negative z axis */
place_location[0].pre_place_approach.direction.vector.z = -1.0;
place_location[0].pre_place_approach.min_distance = 0.095;
place_location[0].pre_place_approach.desired_distance = 0.115;
// Setting post-grasp retreat
// ++++++++++++++++++++++++++
/* Defined with respect to frame_id */
place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0";
/* Direction is set as negative y axis */
place_location[0].post_place_retreat.direction.vector.z = 1.0;
place_location[0].post_place_retreat.min_distance = 0.1;
place_location[0].post_place_retreat.desired_distance = 0.25;
// Setting posture of eef after placing object
// +++++++++++++++++++++++++++++++++++++++++++
/* Similar to the pick case */
openGripper(place_location[0].post_place_posture);
// Set support surface as table2.
group.setSupportSurfaceName("c_table2");
// Call place to place the object using the place locations given.
group.setMaxVelocityScalingFactor(0.05);
group.setMaxAccelerationScalingFactor(0.05);
group.place("c_object", place_location);
// END_SUB_TUTORIAL
}
void addCollisionObjects(moveit::planning_interface::PlanningSceneInterface& planning_scene_interface)
{
// BEGIN_SUB_TUTORIAL table1
//
// Creating Environment
// ^^^^^^^^^^^^^^^^^^^^
// Create vector to hold 4 collision objects.
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.resize(4);
// Add the first table where the cube will originally be kept.
collision_objects[0].id = "c_table1";
collision_objects[0].header.frame_id = "panda_link0";
// Define the primitive and its dimensions.
collision_objects[0].primitives.resize(1);
collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
collision_objects[0].primitives[0].dimensions.resize(3);
collision_objects[0].primitives[0].dimensions[0] = 0.2;
collision_objects[0].primitives[0].dimensions[1] = 0.4;
collision_objects[0].primitives[0].dimensions[2] = 0.2;
// Define the pose of the table.
collision_objects[0].primitive_poses.resize(1);
collision_objects[0].primitive_poses[0].position.x = 0.5;
collision_objects[0].primitive_poses[0].position.y = 0;
collision_objects[0].primitive_poses[0].position.z = 0.1;
// END_SUB_TUTORIAL
collision_objects[0].operation = collision_objects[0].ADD;
// BEGIN_SUB_TUTORIAL table2
// Add the second table where we will be placing the cube.
collision_objects[1].id = "c_table2";
collision_objects[1].header.frame_id = "panda_link0";
// Define the primitive and its dimensions.
collision_objects[1].primitives.resize(1);
collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
collision_objects[1].primitives[0].dimensions.resize(3);
collision_objects[1].primitives[0].dimensions[0] = 0.4;
collision_objects[1].primitives[0].dimensions[1] = 0.2;
collision_objects[1].primitives[0].dimensions[2] = 0.2;
// Define the pose of the table.
collision_objects[1].primitive_poses.resize(1);
collision_objects[1].primitive_poses[0].position.x = 0;
collision_objects[1].primitive_poses[0].position.y = 0.5;
collision_objects[1].primitive_poses[0].position.z = 0.1;
// END_SUB_TUTORIAL
collision_objects[1].operation = collision_objects[1].ADD;
// BEGIN_SUB_TUTORIAL object
// Define the object that we will be manipulating
collision_objects[2].header.frame_id = "panda_link0";
collision_objects[2].id = "c_object";
//Define the primitive and its dimensions.
collision_objects[2].primitives.resize(1);
collision_objects[2].primitives[0].type = collision_objects[1].primitives[0].BOX;
collision_objects[2].primitives[0].dimensions.resize(3);
collision_objects[2].primitives[0].dimensions[0] = 0.02;
collision_objects[2].primitives[0].dimensions[1] = 0.02;
collision_objects[2].primitives[0].dimensions[2] = 0.2;
//Define the pose of the object.
collision_objects[2].primitive_poses.resize(1);
collision_objects[2].primitive_poses[0].position.x = 0.5;
collision_objects[2].primitive_poses[0].position.y = 0;
collision_objects[2].primitive_poses[0].position.z = 0.3;
geometry_msgs::Pose target_pose;
tf2::Quaternion orientation;
orientation.setRPY(0, 0, 0);
geometry_msgs::Pose object_pose;
collision_objects[2].primitive_poses[0].orientation = tf2::toMsg(orientation);
q_pick_object.w = tf2::toMsg(orientation).w;
q_pick_object.x = tf2::toMsg(orientation).x;
q_pick_object.y = tf2::toMsg(orientation).y;
q_pick_object.z = tf2::toMsg(orientation).z;
// END_SUB_TUTORIAL
// the surface
collision_objects[2].operation = collision_objects[2].ADD;
collision_objects[3].header.frame_id = "panda_link0";
collision_objects[3].id = "surface";
// Define the primitive and its dimensions.
collision_objects[3].primitives.resize(1);
collision_objects[3].primitives[0].type = collision_objects[1].primitives[0].BOX;
collision_objects[3].primitives[0].dimensions.resize(3);
collision_objects[3].primitives[0].dimensions[0] = 3.0;
collision_objects[3].primitives[0].dimensions[1] = 3.0;
collision_objects[3].primitives[0].dimensions[2] = 0.1;
// Define the pose of the object.
collision_objects[3].primitive_poses.resize(1);
collision_objects[3].primitive_poses[0].position.x = 0;
collision_objects[3].primitive_poses[0].position.y = 0;
collision_objects[3].primitive_poses[0].position.z = -0.1;
// END_SUB_TUTORIAL
collision_objects[3].operation = collision_objects[3].ADD;
planning_scene_interface.applyCollisionObjects(collision_objects);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "panda_arm_pick_place");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
ros::Duration(5.0).sleep();
ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
if(!ros::param::has("max_grasp_approach_velocity"))
{
ros::param::set("max_grasp_approach_velocity", 0.05);
ros::param::set("max_grasp_approach_acceleration", 0.05);
ros::param::set("max_grasp_transition_velocity", 0.05);
ros::param::set("max_grasp_transition_acceleration", 0.05);
}
ros::WallDuration(1.0).sleep();
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
moveit::planning_interface::MoveGroupInterface group("panda_arm");
group.setPlanningTime(45.0);
addCollisionObjects(planning_scene_interface);
// Wait a bit for ROS things to initialize
ros::WallDuration(1.0).sleep();
pick(group);
ros::WallDuration(1.0).sleep();
place(group);
//std::vector<std::string>objectsToRemove{"object"};
//planning_scene_interface.removeCollisionObjects(objectsToRemove);
ros::waitForShutdown();
return 0;
}
\ No newline at end of file
File moved
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment