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
This is an archived project. Repository and other project resources are read-only.
Show more breadcrumbs
CeTI
ROS
ROS Packages
panda_simulation
Commits
731338a6
Commit
731338a6
authored
5 years ago
by
Sebastian Ebert
Browse files
Options
Downloads
Patches
Plain Diff
added node for constraint path planning (box-based)
parent
e186f737
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
-2
11 additions, 2 deletions
CMakeLists.txt
launch/simulation.launch
+10
-1
10 additions, 1 deletion
launch/simulation.launch
src/constraint_planner.cpp
+104
-0
104 additions, 0 deletions
src/constraint_planner.cpp
with
125 additions
and
3 deletions
CMakeLists.txt
+
11
−
2
View file @
731338a6
...
...
@@ -59,15 +59,24 @@ include_directories(${catkin_INCLUDE_DIRS})
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(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
)
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(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})
#install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
\ No newline at end of file
This diff is collapsed.
Click to expand it.
launch/simulation.launch
+
10
−
1
View file @
731338a6
...
...
@@ -27,7 +27,6 @@
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
<!-- load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
<node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />
...
...
@@ -58,4 +57,14 @@
<!-- run custom node for automatic intialization -->
<node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
<!--<node name="move_group_interface_tutorial" pkg="panda_simulation" type="move_group_interface_tutorial" respawn="false" output="screen"/>-->
<node name="constraint_planner" pkg="panda_simulation" type="constraint_planner" respawn="false" output="screen"/>
<!--<node name="motion_planning_api_tutorial" pkg="panda_simulation" type="motion_planning_api_tutorial" respawn="false" output="screen">
<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
<param name="/planning_plugin" value="ompl_interface/OMPLPlanner"/>
<rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/>
</node>-->
</launch>
This diff is collapsed.
Click to expand it.
src/constraint_planner.cpp
0 → 100644
+
104
−
0
View file @
731338a6
#include
<moveit/move_group_interface/move_group_interface.h>
#include
<moveit/planning_scene_interface/planning_scene_interface.h>
#include
<moveit_msgs/DisplayRobotState.h>
#include
<moveit_msgs/DisplayTrajectory.h>
#include
<moveit_msgs/AttachedCollisionObject.h>
#include
<moveit_msgs/CollisionObject.h>
#include
<moveit_visual_tools/moveit_visual_tools.h>
int
main
(
int
argc
,
char
**
argv
)
{
ros
::
init
(
argc
,
argv
,
"CONSTRAINT PLANNER"
);
ros
::
NodeHandle
node_handle
;
ros
::
AsyncSpinner
spinner
(
1
);
spinner
.
start
();
static
const
std
::
string
PLANNING_GROUP
=
"panda_arm"
;
moveit
::
planning_interface
::
MoveGroupInterface
move_group
(
PLANNING_GROUP
);
moveit
::
planning_interface
::
PlanningSceneInterface
planning_scene_interface
;
const
robot_state
::
JointModelGroup
*
joint_model_group
=
move_group
.
getCurrentState
()
->
getJointModelGroup
(
PLANNING_GROUP
);
// Visualization Setup
namespace
rvt
=
rviz_visual_tools
;
moveit_visual_tools
::
MoveItVisualTools
visual_tools
(
"panda_link0"
);
visual_tools
.
deleteAllMarkers
();
visual_tools
.
loadRemoteControl
();
Eigen
::
Isometry3d
text_pose
=
Eigen
::
Isometry3d
::
Identity
();
text_pose
.
translation
().
z
()
=
1.75
;
visual_tools
.
publishText
(
text_pose
,
"Sample constraint planner node"
,
rvt
::
WHITE
,
rvt
::
XLARGE
);
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools
.
trigger
();
// Getting Basic Information
ROS_INFO_NAMED
(
"constraint_planner"
,
"Planning frame: %s"
,
move_group
.
getPlanningFrame
().
c_str
());
ROS_INFO_NAMED
(
"constraint_planner"
,
"End effector link: %s"
,
move_group
.
getEndEffectorLink
().
c_str
());
ROS_INFO_NAMED
(
"constraint_planner"
,
"Available Planning Groups:"
);
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
);
// 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
visual_tools
.
publishText
(
text_pose
,
"Add object"
,
rvt
::
WHITE
,
rvt
::
XLARGE
);
visual_tools
.
trigger
();
visual_tools
.
prompt
(
"Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"
);
// Now when we plan a trajectory it will avoid the obstacle
move_group
.
setStartState
(
*
move_group
.
getCurrentState
());
geometry_msgs
::
Pose
another_pose
;
another_pose
.
orientation
.
w
=
1.0
;
another_pose
.
position
.
x
=
0.4
;
another_pose
.
position
.
y
=
-
0.4
;
another_pose
.
position
.
z
=
0.9
;
move_group
.
setPoseTarget
(
another_pose
);
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"
);
visual_tools
.
prompt
(
"Press 'next' to move the simulated robot."
);
visual_tools
.
trigger
();
// Move the simulated robot in gazebo
move_group
.
move
();
ros
::
shutdown
();
return
0
;
}
\ No newline at end of file
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