Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
S
sample_applications
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package registry
Container registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Christian Braun
sample_applications
Commits
d18e1e64
Commit
d18e1e64
authored
4 years ago
by
Johannes Mey
Browse files
Options
Downloads
Patches
Plain Diff
pick and place working in simulation
parent
7ef30876
Branches
Branches containing commit
No related tags found
No related merge requests found
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
CMakeLists.txt
+7
-1
7 additions, 1 deletion
CMakeLists.txt
launch/pick_place_simulation.launch
+13
-0
13 additions, 0 deletions
launch/pick_place_simulation.launch
package.xml
+1
-0
1 addition, 0 deletions
package.xml
src/pick_place_tutorial.cpp
+349
-0
349 additions, 0 deletions
src/pick_place_tutorial.cpp
with
370 additions
and
1 deletion
CMakeLists.txt
+
7
−
1
View file @
d18e1e64
...
...
@@ -24,7 +24,8 @@ find_package(catkin REQUIRED
moveit_ros_planning
moveit_ros_planning_interface
controller_interface
hardware_interface
)
hardware_interface
simulation_util
)
# System dependencies are found with CMake's conventions
find_package
(
PkgConfig REQUIRED
)
...
...
@@ -40,6 +41,7 @@ catkin_package(CATKIN_DEPENDS
controller_interface
hardware_interface
pluginlib
simulation_util
DEPENDS
# system_lib
)
...
...
@@ -55,9 +57,13 @@ add_executable(SampleConstraintPlanner src/SampleConstraintPlanner.cpp)
add_executable
(
SampleTimedCartesianPlanner src/SampleTimedCartesianPlanner.cpp
)
add_executable
(
SampleSimpleMotion src/SampleSimpleMotion.cpp
)
add_executable
(
MinimalSimpleMotion src/MinimalSimpleMotion.cpp
)
add_executable
(
${
PROJECT_NAME
}
_pick_place src/pick_place_tutorial.cpp
)
set_target_properties
(
${
PROJECT_NAME
}
_pick_place PROPERTIES OUTPUT_NAME pick_place PREFIX
""
)
# Specify libraries to link a library or executable target against
target_link_libraries
(
SampleConstraintPlanner
${
catkin_LIBRARIES
}
)
target_link_libraries
(
SampleTimedCartesianPlanner
${
catkin_LIBRARIES
}
)
target_link_libraries
(
SampleSimpleMotion
${
catkin_LIBRARIES
}
)
target_link_libraries
(
MinimalSimpleMotion
${
catkin_LIBRARIES
}
)
target_link_libraries
(
${
PROJECT_NAME
}
_pick_place
${
catkin_LIBRARIES
}
)
This diff is collapsed.
Click to expand it.
launch/pick_place_simulation.launch
0 → 100644
+
13
−
0
View file @
d18e1e64
<launch>
<include file="$(find panda_simulation)/launch/simulation.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 name="rqt_console" pkg="rqt_console" type="rqt_console" />-->
</launch>
This diff is collapsed.
Click to expand it.
package.xml
+
1
−
0
View file @
d18e1e64
...
...
@@ -40,6 +40,7 @@
<depend>
moveit_visual_tools
</depend>
<depend>
controller_interface
</depend>
<depend>
hardware_interface
</depend>
<depend>
simulation_util
</depend>
<exec_depend>
pluginlib
</exec_depend>
...
...
This diff is collapsed.
Click to expand it.
src/pick_place_tutorial.cpp
0 → 100644
+
349
−
0
View file @
d18e1e64
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2012, Willow Garage, Inc.
* 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 Willow Garage 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: Ioan Sucan, Ridhwan Luthra*/
// 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>
// Zone Spawner
#include
<GazeboZoneSpawner.h>
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.00
;
posture
.
points
[
0
].
positions
[
1
]
=
0.00
;
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|
// Make sure that when you set the grasp_pose, you are setting it to be the pose of the last link in
// your manipulator which in this case would be `"panda_link8"` You will have to compensate for the
// transform from `"panda_link8"` to the palm of the end effector.
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.4
;
grasps
[
0
].
grasp_pose
.
pose
.
position
.
y
=
0
;
grasps
[
0
].
grasp_pose
.
pose
.
position
.
z
=
0.5
;
// 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
.
x
=
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
;
// 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
(
"table1"
);
// Call pick to pick up the object using the grasps given
move_group
.
pick
(
"object"
,
grasps
);
// 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. |br|
// |br|
// Ideally, you would create a vector of place locations to be attempted although in this example, we only create
// a 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
);
/* For place location, we set the value to 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.5
;
// 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
.
y
=
-
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
(
"table2"
);
// Call place to place the object using the place locations given.
group
.
place
(
"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 3 collision objects.
std
::
vector
<
moveit_msgs
::
CollisionObject
>
collision_objects
;
collision_objects
.
resize
(
3
);
// Add the first table where the cube will originally be kept.
collision_objects
[
0
].
id
=
"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.4
;
/* 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.2
;
collision_objects
[
0
].
primitive_poses
[
0
].
orientation
.
w
=
1.0
;
shape_msgs
::
SolidPrimitive
table1
;
table1
.
type
=
shape_msgs
::
SolidPrimitive
::
BOX
;
table1
.
dimensions
=
collision_objects
[
0
].
primitives
[
0
].
dimensions
;
geometry_msgs
::
Pose
table1pose
;
table1pose
.
position
.
x
=
collision_objects
[
0
].
primitive_poses
[
0
].
position
.
x
;
table1pose
.
position
.
y
=
collision_objects
[
0
].
primitive_poses
[
0
].
position
.
y
;
table1pose
.
position
.
z
=
collision_objects
[
0
].
primitive_poses
[
0
].
position
.
z
;
// the orientation is always the default
table1pose
.
orientation
.
w
=
1
;
GazeboZoneSpawner
::
spawnPrimitive
(
"table1"
,
table1
,
table1pose
,
20
,
true
);
// 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
=
"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.4
;
/* 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.2
;
collision_objects
[
1
].
primitive_poses
[
0
].
orientation
.
w
=
1.0
;
shape_msgs
::
SolidPrimitive
table2
;
table2
.
type
=
shape_msgs
::
SolidPrimitive
::
BOX
;
table2
.
dimensions
=
collision_objects
[
1
].
primitives
[
0
].
dimensions
;
geometry_msgs
::
Pose
table2pose
;
table2pose
.
position
.
x
=
collision_objects
[
1
].
primitive_poses
[
0
].
position
.
x
;
table2pose
.
position
.
y
=
collision_objects
[
1
].
primitive_poses
[
0
].
position
.
y
;
table2pose
.
position
.
z
=
collision_objects
[
1
].
primitive_poses
[
0
].
position
.
z
;
// the orientation is always the default
table2pose
.
orientation
.
w
=
1
;
GazeboZoneSpawner
::
spawnPrimitive
(
"table2"
,
table2
,
table2pose
,
20
,
true
);
// 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
=
"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.5
;
collision_objects
[
2
].
primitive_poses
[
0
].
orientation
.
w
=
1.0
;
shape_msgs
::
SolidPrimitive
object
;
object
.
type
=
shape_msgs
::
SolidPrimitive
::
BOX
;
object
.
dimensions
=
collision_objects
[
2
].
primitives
[
0
].
dimensions
;
geometry_msgs
::
Pose
objectpose
;
objectpose
.
position
.
x
=
collision_objects
[
2
].
primitive_poses
[
0
].
position
.
x
;
objectpose
.
position
.
y
=
collision_objects
[
2
].
primitive_poses
[
0
].
position
.
y
;
objectpose
.
position
.
z
=
collision_objects
[
2
].
primitive_poses
[
0
].
position
.
z
;
// the orientation is always the default
objectpose
.
orientation
.
w
=
1
;
GazeboZoneSpawner
::
spawnPrimitive
(
"pickObject"
,
object
,
objectpose
,
1
);
// END_SUB_TUTORIAL
collision_objects
[
2
].
operation
=
collision_objects
[
2
].
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
);
ROS_INFO_NAMED
(
"constraint_planner"
,
">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<"
);
ros
::
Duration
(
5.0
).
sleep
();
ROS_INFO_NAMED
(
"constraint_planner"
,
">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<"
);
spinner
.
start
();
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
);
ros
::
waitForShutdown
();
return
0
;
}
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment