Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
P
panda_simulation
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
Container registry
Model registry
Operate
Environments
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
Nikhil Ambardar
panda_simulation
Commits
b162c39b
Commit
b162c39b
authored
5 years ago
by
Sebastian Ebert
Browse files
Options
Downloads
Patches
Plain Diff
centralized safety-zone parameterizable by width / height / depth
parent
731338a6
No related branches found
No related tags found
No related merge requests found
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
CMakeLists.txt
+11
-16
11 additions, 16 deletions
CMakeLists.txt
src/SafetyEnvironmentCreator.cpp
+217
-0
217 additions, 0 deletions
src/SafetyEnvironmentCreator.cpp
src/constraint_planner.cpp
+16
-30
16 additions, 30 deletions
src/constraint_planner.cpp
with
244 additions
and
46 deletions
CMakeLists.txt
+
11
−
16
View file @
b162c39b
...
...
@@ -56,27 +56,22 @@ catkin_package(CATKIN_DEPENDS
# Specify additional locations of header files Your package locations should be listed before other locations
include_directories
(
${
catkin_INCLUDE_DIRS
}
)
# add custom controller as library
add_library
(
${
PROJECT_NAME
}
_controllers_lib src/joint_position_controller.cpp
)
#add_library(SafetyEnvironmentCreator src/SafetyEnvironmentCreator.cpp)
# Specify libraries to link a library or executable target against
target_link_libraries
(
${
PROJECT_NAME
}
_controllers_lib
${
catkin_LIBRARIES
}
)
#target_link_libraries(SafetyEnvironmentCreator ${catkin_LIBRARIES} ${Boost_LIBRARIES})
add_executable
(
robot_control_node src/robot_control_node.cpp
)
add_executable
(
box_publisher_node src/box_publisher.cpp
)
add_executable
(
robot_state_initializer_node src/robot_state_initializer.cpp
)
#add_executable(move_group_interface_tutorial src/move_group_interface_tutorial.cpp)
#add_executable(motion_planning_api_tutorial src/motion_planning_api_tutorial.cpp)
add_executable
(
constraint_planner src/constraint_planner.cpp
)
add_executable
(
constraint_planner src/constraint_planner.cpp src/SafetyEnvironmentCreator.cpp
)
target_link_libraries
(
robot_control_node
${
catkin_LIBRARIES
}
boost_filesystem
${
JSONCPP_LIBRARIES
}
)
target_link_libraries
(
box_publisher_node
${
catkin_LIBRARIES
}
)
target_link_libraries
(
robot_state_initializer_node
${
catkin_LIBRARIES
}
)
#target_link_libraries(move_group_interface_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES})
#target_link_libraries(motion_planning_api_tutorial ${catkin_LIBRARIES} ${Boost_LIBRARIES})
target_link_libraries
(
constraint_planner
${
catkin_LIBRARIES
}
${
Boost_LIBRARIES
}
)
# add custom controller as library
add_library
(
${
PROJECT_NAME
}
_controllers_lib src/joint_position_controller.cpp
)
# Specify libraries to link a library or executable target against
target_link_libraries
(
${
PROJECT_NAME
}
_controllers_lib
${
catkin_LIBRARIES
}
)
#install(TARGETS move_group_interface_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
#install(TARGETS motion_planning_api_tutorial DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
target_link_libraries
(
constraint_planner
${
catkin_LIBRARIES
}
)
#target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator)
#install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
\ No newline at end of file
This diff is collapsed.
Click to expand it.
src/SafetyEnvironmentCreator.cpp
0 → 100644
+
217
−
0
View file @
b162c39b
//
// Created by Sebastian Ebert
//
#include
<moveit/move_group_interface/move_group_interface.h>
#include
<moveit_msgs/CollisionObject.h>
#include
<moveit_visual_tools/moveit_visual_tools.h>
class
SafetyEnvironmentCreator
{
private:
double
safety_zone_width
;
double
safety_zone_height
;
double
safety_zone_depth
;
public:
// ctor
SafetyEnvironmentCreator
(
double
zone_width
,
double
zone_height
,
double
zone_depth
)
{
safety_zone_width
=
zone_width
;
safety_zone_height
=
zone_height
;
safety_zone_depth
=
zone_depth
;
}
double
getZoneWidth
()
{
return
safety_zone_width
;
}
double
getZoneHeight
()
{
return
safety_zone_height
;
}
double
getZoneDepth
()
{
return
safety_zone_depth
;
}
std
::
vector
<
moveit_msgs
::
CollisionObject
>
createCentralizedSafetyEnvironment
(
moveit
::
planning_interface
::
MoveGroupInterface
&
move_group
)
{
std
::
vector
<
moveit_msgs
::
CollisionObject
>
collision_objects
;
// OBJECT 1 ( left box / -y )
// ^^^^^^^^
// Define a collision object ROS message.
moveit_msgs
::
CollisionObject
collision_object
;
collision_object
.
header
.
frame_id
=
move_group
.
getPlanningFrame
();
// The id of the object is used to identify it.
collision_object
.
id
=
"box1"
;
// Define a box to add to the world.
shape_msgs
::
SolidPrimitive
primitive
;
primitive
.
type
=
primitive
.
BOX
;
primitive
.
dimensions
.
resize
(
3
);
primitive
.
dimensions
[
0
]
=
getZoneDepth
()
+
0.2
;
// x
primitive
.
dimensions
[
1
]
=
0.1
;
// y
primitive
.
dimensions
[
2
]
=
getZoneHeight
();
// z
// Define a pose for the box (specified relative to frame_id)
geometry_msgs
::
Pose
box_pose
;
box_pose
.
orientation
.
w
=
1.0
;
box_pose
.
position
.
x
=
0.0
;
box_pose
.
position
.
y
=
-
1
*
((
getZoneWidth
()
/
2
)
+
0.05
);
box_pose
.
position
.
z
=
primitive
.
dimensions
[
2
]
/
2
;
collision_object
.
primitives
.
push_back
(
primitive
);
collision_object
.
primitive_poses
.
push_back
(
box_pose
);
collision_object
.
operation
=
collision_object
.
ADD
;
collision_objects
.
push_back
(
collision_object
);
// OBJECT 2 right box / +y )
// ^^^^^^^^
moveit_msgs
::
CollisionObject
collision_object_2
;
collision_object_2
.
header
.
frame_id
=
move_group
.
getPlanningFrame
();
// The id of the object is used to identify it.
collision_object_2
.
id
=
"box2"
;
// Define a box to add to the world.
shape_msgs
::
SolidPrimitive
primitive_2
;
primitive_2
.
type
=
primitive
.
BOX
;
primitive_2
.
dimensions
.
resize
(
3
);
primitive_2
.
dimensions
[
0
]
=
getZoneDepth
()
+
0.2
;
primitive_2
.
dimensions
[
1
]
=
0.1
;
primitive_2
.
dimensions
[
2
]
=
getZoneHeight
();
// Define a pose for the box (specified relative to frame_id)
geometry_msgs
::
Pose
box_pose_2
;
box_pose_2
.
orientation
.
w
=
1.0
;
box_pose_2
.
position
.
x
=
0.0
;
box_pose_2
.
position
.
y
=
((
getZoneWidth
()
/
2
)
+
0.05
);
box_pose_2
.
position
.
z
=
primitive_2
.
dimensions
[
2
]
/
2
;
collision_object_2
.
primitives
.
push_back
(
primitive_2
);
collision_object_2
.
primitive_poses
.
push_back
(
box_pose_2
);
collision_object_2
.
operation
=
collision_object_2
.
ADD
;
collision_objects
.
push_back
(
collision_object_2
);
// OBJECT 3 (front box / +x)
// ^^^^^^^^
moveit_msgs
::
CollisionObject
collision_object_3
;
collision_object_3
.
header
.
frame_id
=
move_group
.
getPlanningFrame
();
// The id of the object is used to identify it.
collision_object_3
.
id
=
"box3"
;
// Define a box to add to the world.
shape_msgs
::
SolidPrimitive
primitive_3
;
primitive_3
.
type
=
primitive
.
BOX
;
primitive_3
.
dimensions
.
resize
(
3
);
primitive_3
.
dimensions
[
0
]
=
0.1
;
primitive_3
.
dimensions
[
1
]
=
getZoneWidth
()
+
0.2
;
primitive_3
.
dimensions
[
2
]
=
getZoneHeight
();
// Define a pose for the box (specified relative to frame_id)
geometry_msgs
::
Pose
box_pose_3
;
box_pose_3
.
orientation
.
w
=
1.0
;
box_pose_3
.
position
.
x
=
((
getZoneDepth
()
/
2
)
+
0.05
);
box_pose_3
.
position
.
y
=
0.0
;
box_pose_3
.
position
.
z
=
primitive_3
.
dimensions
[
2
]
/
2
;
collision_object_3
.
primitives
.
push_back
(
primitive_3
);
collision_object_3
.
primitive_poses
.
push_back
(
box_pose_3
);
collision_object_3
.
operation
=
collision_object_3
.
ADD
;
collision_objects
.
push_back
(
collision_object_3
);
// OBJECT 4 (back box / -x)
// ^^^^^^^^
moveit_msgs
::
CollisionObject
collision_object_4
;
collision_object_4
.
header
.
frame_id
=
move_group
.
getPlanningFrame
();
// The id of the object is used to identify it.
collision_object_4
.
id
=
"box4"
;
// Define a box to add to the world.
shape_msgs
::
SolidPrimitive
primitive_4
;
primitive_4
.
type
=
primitive
.
BOX
;
primitive_4
.
dimensions
.
resize
(
3
);
primitive_4
.
dimensions
[
0
]
=
0.1
;
primitive_4
.
dimensions
[
1
]
=
getZoneWidth
()
+
0.2
;
primitive_4
.
dimensions
[
2
]
=
getZoneHeight
();
// Define a pose for the box (specified relative to frame_id)
geometry_msgs
::
Pose
box_pose_4
;
box_pose_4
.
orientation
.
w
=
1.0
;
box_pose_4
.
position
.
x
=
-
1
*
((
getZoneDepth
()
/
2
)
+
0.05
);
box_pose_4
.
position
.
y
=
0.0
;
box_pose_4
.
position
.
z
=
primitive_4
.
dimensions
[
2
]
/
2
;
collision_object_4
.
primitives
.
push_back
(
primitive_4
);
collision_object_4
.
primitive_poses
.
push_back
(
box_pose_4
);
collision_object_4
.
operation
=
collision_object_4
.
ADD
;
collision_objects
.
push_back
(
collision_object_4
);
// OBJECT 5 (floor / -z)
// ^^^^^^^^
moveit_msgs
::
CollisionObject
collision_object_5
;
collision_object_5
.
header
.
frame_id
=
move_group
.
getPlanningFrame
();
// The id of the object is used to identify it.
collision_object_5
.
id
=
"box5"
;
// Define a box to add to the world.
shape_msgs
::
SolidPrimitive
primitive_5
;
primitive_5
.
type
=
primitive
.
BOX
;
primitive_5
.
dimensions
.
resize
(
3
);
primitive_5
.
dimensions
[
0
]
=
getZoneDepth
()
+
0.2
;
primitive_5
.
dimensions
[
1
]
=
getZoneWidth
()
+
0.2
;
primitive_5
.
dimensions
[
2
]
=
0.1
;
// Define a pose for the box (specified relative to frame_id)
geometry_msgs
::
Pose
box_pose_5
;
box_pose_5
.
orientation
.
w
=
1.0
;
box_pose_5
.
position
.
x
=
0.0
;
box_pose_5
.
position
.
y
=
0.0
;
box_pose_5
.
position
.
z
=
-
0.05
;
collision_object_5
.
primitives
.
push_back
(
primitive_5
);
collision_object_5
.
primitive_poses
.
push_back
(
box_pose_5
);
collision_object_5
.
operation
=
collision_object_5
.
ADD
;
collision_objects
.
push_back
(
collision_object_5
);
// OBJECT 5 (floor / -z)
// ^^^^^^^^
moveit_msgs
::
CollisionObject
collision_object_6
;
collision_object_6
.
header
.
frame_id
=
move_group
.
getPlanningFrame
();
// The id of the object is used to identify it.
collision_object_6
.
id
=
"box5"
;
// Define a box to add to the world.
shape_msgs
::
SolidPrimitive
primitive_6
;
primitive_6
.
type
=
primitive
.
BOX
;
primitive_6
.
dimensions
.
resize
(
3
);
primitive_6
.
dimensions
[
0
]
=
getZoneDepth
()
+
0.2
;
primitive_6
.
dimensions
[
1
]
=
getZoneWidth
()
+
0.2
;
primitive_6
.
dimensions
[
2
]
=
0.1
;
// Define a pose for the box (specified relative to frame_id)
geometry_msgs
::
Pose
box_pose_6
;
box_pose_6
.
orientation
.
w
=
1.0
;
box_pose_6
.
position
.
x
=
0.0
;
box_pose_6
.
position
.
y
=
0.0
;
box_pose_6
.
position
.
z
=
getZoneHeight
()
+
0.05
;
collision_object_6
.
primitives
.
push_back
(
primitive_6
);
collision_object_6
.
primitive_poses
.
push_back
(
box_pose_6
);
collision_object_6
.
operation
=
collision_object_6
.
ADD
;
collision_objects
.
push_back
(
collision_object_6
);
return
collision_objects
;
}
};
\ No newline at end of file
This diff is collapsed.
Click to expand it.
src/constraint_planner.cpp
+
16
−
30
View file @
b162c39b
...
...
@@ -6,9 +6,10 @@
#include
<moveit_msgs/AttachedCollisionObject.h>
#include
<moveit_msgs/CollisionObject.h>
#include
<moveit_visual_tools/moveit_visual_tools.h>
#include
"SafetyEnvironmentCreator.cpp"
int
main
(
int
argc
,
char
**
argv
)
{
ros
::
init
(
argc
,
argv
,
"CONSTRAINT PLANNER"
);
...
...
@@ -25,8 +26,9 @@ int main(int argc, char** argv)
// Visualization Setup
namespace
rvt
=
rviz_visual_tools
;
moveit_visual_tools
::
MoveItVisualTools
visual_tools
(
"panda_link0"
);
visual_tools
.
deleteAllMarkers
();
//
visual_tools.deleteAllMarkers();
visual_tools
.
loadRemoteControl
();
visual_tools
.
setAlpha
(
0.5
);
Eigen
::
Isometry3d
text_pose
=
Eigen
::
Isometry3d
::
Identity
();
text_pose
.
translation
().
z
()
=
1.75
;
...
...
@@ -42,37 +44,15 @@ int main(int argc, char** argv)
std
::
copy
(
move_group
.
getJointModelGroupNames
().
begin
(),
move_group
.
getJointModelGroupNames
().
end
(),
std
::
ostream_iterator
<
std
::
string
>
(
std
::
cout
,
", "
));
// Define a collision object ROS message.
moveit_msgs
::
CollisionObject
collision_object
;
collision_object
.
header
.
frame_id
=
move_group
.
getPlanningFrame
();
// The id of the object is used to identify it.
collision_object
.
id
=
"box1"
;
// Define a box to add to the world.
shape_msgs
::
SolidPrimitive
primitive
;
primitive
.
type
=
primitive
.
BOX
;
primitive
.
dimensions
.
resize
(
3
);
primitive
.
dimensions
[
0
]
=
0.4
;
primitive
.
dimensions
[
1
]
=
0.1
;
primitive
.
dimensions
[
2
]
=
0.4
;
// Define a pose for the box (specified relative to frame_id)
geometry_msgs
::
Pose
box_pose
;
box_pose
.
orientation
.
w
=
1.0
;
box_pose
.
position
.
x
=
0.4
;
box_pose
.
position
.
y
=
-
0.2
;
box_pose
.
position
.
z
=
1.0
;
collision_object
.
primitives
.
push_back
(
primitive
);
collision_object
.
primitive_poses
.
push_back
(
box_pose
);
collision_object
.
operation
=
collision_object
.
ADD
;
std
::
vector
<
moveit_msgs
::
CollisionObject
>
collision_objects
;
collision_objects
.
push_back
(
collision_object
);
// Safety Box
SafetyEnvironmentCreator
sec
(
2.0
,
2.0
,
2.0
);
std
::
vector
<
moveit_msgs
::
CollisionObject
>
collision_objects
=
sec
.
createCentralizedSafetyEnvironment
(
move_group
);
//visual_tools.processCollisionObjectMsg(collision_objects[2], rviz_visual_tools::RED);
// visual_tools.setAlpha(0.0);
// Now, let's add the collision object into the world
ROS_INFO_NAMED
(
"tutorial"
,
"Add an object into the world"
);
planning_scene_interface
.
addCollisionObjects
(
collision_objects
);
// Show text in RViz of status
...
...
@@ -89,6 +69,12 @@ int main(int argc, char** argv)
another_pose
.
position
.
z
=
0.9
;
move_group
.
setPoseTarget
(
another_pose
);
//moveit_msgs::MotionPlanRequest mr;
//mr.max_velocity_scaling_factor = 0.5;
//move_group.constructMotionPlanRequest(mr);
move_group
.
setMaxVelocityScalingFactor
(
0.1
);
move_group
.
setWorkspace
(
-
0.5
,
-
0.5
,
-
0.5
,
0
,
0
,
0
);
moveit
::
planning_interface
::
MoveGroupInterface
::
Plan
my_plan
;
bool
success
=
(
move_group
.
plan
(
my_plan
)
==
moveit
::
planning_interface
::
MoveItErrorCode
::
SUCCESS
);
ROS_INFO_NAMED
(
"constraint_planner"
,
"Visualizing constraint plan %s"
,
success
?
""
:
"FAILED"
);
...
...
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