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

align with repo tag 0.8.1

parent e6757bb9
No related branches found
No related tags found
No related merge requests found
Showing
with 193 additions and 583 deletions
moveit_setup_assistant_config:
URDF:
package: franka_description
relative_path: robots/panda_arm.urdf.xacro
relative_path: robots/panda/panda.urdf.xacro
xacro_args: hand:=true
SRDF:
relative_path: config/panda.srdf.xacro
......
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package panda_moveit_config
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.8.1 (2022-09-11)
------------------
* Update to franka_description 0.10.0 (`#119 <https://github.com/ros-planning/panda_moveit_config/issues/119>`_)
* Example use of ns parameter in sensors.yaml (`#88 <https://github.com/ros-planning/panda_moveit_config/issues/88>`_)
* Drop link8 from ACM as this link doesn't have any collision model anymore
* srdf: Use loop macros to reduce code redundancy
* Unify calls to xacro
* Contributors: Matt Droter, Robert Haschke, Tim Redick
0.8.0 (2022-09-01)
------------------
* Thorough rework based on franka_description 0.9.1 (using fine and coarse collision models)
The internal robot controller uses coarse collision models for self-collision checking.
In MoveIt, these coarse models should be used for self-collision checking only as well.
Particularly, these coarse models should not be used for collision checking with the environment.
......@@ -7,67 +7,67 @@
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
<group name="${name}">
<chain base_link="panda_link0" tip_link="${tip_link}" />
<chain base_link="$(arg arm_id)_link0" tip_link="${tip_link}" />
</group>
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
<group_state name="ready" group="${name}">
<joint name="panda_joint1" value="0" />
<joint name="panda_joint2" value="${-pi/4}" />
<joint name="panda_joint3" value="0" />
<joint name="panda_joint4" value="${-3*pi/4}" />
<joint name="panda_joint5" value="0" />
<joint name="panda_joint6" value="${pi/2}" />
<joint name="panda_joint7" value="${pi/4}" />
<joint name="$(arg arm_id)_joint1" value="0" />
<joint name="$(arg arm_id)_joint2" value="${-pi/4}" />
<joint name="$(arg arm_id)_joint3" value="0" />
<joint name="$(arg arm_id)_joint4" value="${-3*pi/4}" />
<joint name="$(arg arm_id)_joint5" value="0" />
<joint name="$(arg arm_id)_joint6" value="${pi/2}" />
<joint name="$(arg arm_id)_joint7" value="${pi/4}" />
</group_state>
<group_state name="extended" group="${name}">
<joint name="panda_joint1" value="0" />
<joint name="panda_joint2" value="0" />
<joint name="panda_joint3" value="0" />
<joint name="panda_joint4" value="-0.1" />
<joint name="panda_joint5" value="0" />
<joint name="panda_joint6" value="${pi}" />
<joint name="panda_joint7" value="${pi/4}" />
<joint name="$(arg arm_id)_joint1" value="0" />
<joint name="$(arg arm_id)_joint2" value="0" />
<joint name="$(arg arm_id)_joint3" value="0" />
<joint name="$(arg arm_id)_joint4" value="-0.1" />
<joint name="$(arg arm_id)_joint5" value="0" />
<joint name="$(arg arm_id)_joint6" value="${pi}" />
<joint name="$(arg arm_id)_joint7" value="${pi/4}" />
</group_state>
</xacro:macro>
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="panda_link0" />
<virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="$(arg arm_id)_link0" />
<xacro:macro name="collision" params="link">
<!-- Enable (environmental) collisions of ${link}_sc -->
<disable_default_collisions link="${link}_sc" />
<!-- Disable collisions of link with any other arm link, as these are handled by the "sc" links -->
<disable_collisions link1="${link}" link2="panda_link0" reason="Never" />
<disable_collisions link1="${link}" link2="panda_link1" reason="Never" />
<disable_collisions link1="${link}" link2="panda_link2" reason="Never" />
<disable_collisions link1="${link}" link2="panda_link3" reason="Never" />
<disable_collisions link1="${link}" link2="panda_link4" reason="Never" />
<disable_collisions link1="${link}" link2="panda_link5" reason="Never" />
<disable_collisions link1="${link}" link2="panda_link6" reason="Never" />
<disable_collisions link1="${link}" link2="panda_link7" reason="Never" />
<disable_collisions link1="${link}" link2="panda_link8" reason="Never" />
<xacro:macro name="disable_collisions_for" params="link:=^ others:=^">
<xacro:if value="${others}">
<xacro:property name="other" value="${others.pop(0)}" />
<disable_collisions link1="${link}" link2="${other}" reason="Never" />
<!-- recursively call -->
<xacro:disable_collisions_for />
</xacro:if>
</xacro:macro>
<xacro:collision link="panda_link0"/>
<enable_collisions link1="panda_link0_sc" link2="panda_link5_sc" />
<enable_collisions link1="panda_link0_sc" link2="panda_link6_sc" />
<enable_collisions link1="panda_link0_sc" link2="panda_link7_sc" />
<enable_collisions link1="panda_link0_sc" link2="panda_link8_sc" />
<xacro:collision link="panda_link1" />
<enable_collisions link1="panda_link1_sc" link2="panda_link5_sc" />
<enable_collisions link1="panda_link1_sc" link2="panda_link6_sc" />
<enable_collisions link1="panda_link1_sc" link2="panda_link7_sc" />
<enable_collisions link1="panda_link1_sc" link2="panda_link8_sc" />
<xacro:collision link="panda_link2" />
<enable_collisions link1="panda_link2_sc" link2="panda_link5_sc" />
<enable_collisions link1="panda_link2_sc" link2="panda_link6_sc" />
<enable_collisions link1="panda_link2_sc" link2="panda_link7_sc" />
<enable_collisions link1="panda_link2_sc" link2="panda_link8_sc" />
<xacro:collision link="panda_link3" />
<enable_collisions link1="panda_link3_sc" link2="panda_link7_sc" />
<enable_collisions link1="panda_link3_sc" link2="panda_link8_sc" />
<xacro:collision link="panda_link4" />
<xacro:collision link="panda_link5" />
<xacro:collision link="panda_link6" />
<xacro:collision link="panda_link7" />
<xacro:collision link="panda_link8" />
<xacro:macro name="enable_collisions_for" params="link:=^ others:=^">
<xacro:if value="${others}">
<xacro:property name="other" value="${others.pop(0)}" />
<enable_collisions link1="${link}" link2="${other}" />
<!-- recursively call -->
<xacro:enable_collisions_for />
</xacro:if>
</xacro:macro>
<xacro:macro name="configure_collisions" params="link enabled:=${[]}">
<!-- Disable collision checking between normal links, as these are handled by "sc" links -->
<xacro:property name="link_fmt" value="$(arg arm_id)_link{}" />
<xacro:disable_collisions_for link="${link_fmt.format(link)}" others="${[link_fmt.format(i) for i in python.range(8)]}" />
<!-- Disable all collision checking for ${link}_sc -->
<xacro:property name="link_fmt" value="$(arg arm_id)_link{}_sc" />
<disable_default_collisions link="${link_fmt.format(link)}" />
<!-- Re-enable collisions checking for selected links -->
<xacro:enable_collisions_for link="${link_fmt.format(link)}" others="${[link_fmt.format(i) for i in enabled]}" />
</xacro:macro>
<!-- Configure ACM -->
<xacro:configure_collisions link="0" enabled="${[5,6,7]}" />
<xacro:configure_collisions link="1" enabled="${[5,6,7]}" />
<xacro:configure_collisions link="2" enabled="${[5,6,7]}" />
<xacro:configure_collisions link="3" enabled="${[7]}" />
<xacro:configure_collisions link="4" />
<xacro:configure_collisions link="5" />
<xacro:configure_collisions link="6" />
<xacro:configure_collisions link="7" />
</robot>
controller_list:
- name: fake_panda_arm_controller
- name: fake_$(arg arm_id)_arm_controller
type: $(arg fake_execution_type)
joints:
- panda_joint1
- panda_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7
- name: fake_hand_controller
- $(arg arm_id)_joint1
- $(arg arm_id)_joint2
- $(arg arm_id)_joint3
- $(arg arm_id)_joint4
- $(arg arm_id)_joint5
- $(arg arm_id)_joint6
- $(arg arm_id)_joint7
- name: fake_$(arg arm_id)_hand_controller
type: $(arg fake_execution_type)
joints:
- panda_finger_joint1
- name: fake_panda_arm_hand_controller
type: $(arg fake_execution_type)
joints:
- panda_joint1
- panda_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7
- panda_finger_joint1
- $(arg arm_id)_finger_joint1
initial: # Define initial robot poses per group
- group: panda_arm
- group: $(arg arm_id)_arm
pose: ready
- group: hand
pose: open
\ No newline at end of file
- group: $(arg arm_id)_hand
pose: open
<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
<xacro:macro name="finger_collisions" params="finger">
<!-- Disable all self-collisions between finger and arm links,
as these are already covered by the coarser hand collision model -->
<disable_collisions link1="${finger}" link2="panda_link0" reason="Never" />
<disable_collisions link1="${finger}" link2="panda_link1" reason="Never" />
<disable_collisions link1="${finger}" link2="panda_link2" reason="Never" />
<disable_collisions link1="${finger}" link2="panda_link3" reason="Never" />
<disable_collisions link1="${finger}" link2="panda_link4" reason="Never" />
<disable_collisions link1="${finger}" link2="panda_link5" reason="Never" />
<disable_collisions link1="${finger}" link2="panda_link6" reason="Never" />
<disable_collisions link1="${finger}" link2="panda_link7" reason="Never" />
<disable_collisions link1="${finger}" link2="panda_link8" reason="Never" />
<disable_collisions link1="${finger}" link2="panda_hand" reason="Never" />
</xacro:macro>
<xacro:macro name="hand">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
<group name="hand">
<link name="panda_hand"/>
<link name="panda_leftfinger"/>
<link name="panda_rightfinger"/>
<group name="$(arg arm_id)_hand">
<link name="$(arg arm_id)_hand"/>
<link name="$(arg arm_id)_leftfinger"/>
<link name="$(arg arm_id)_rightfinger"/>
</group>
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
<group_state name="open" group="hand">
<joint name="panda_finger_joint1" value="0.035"/>
<joint name="panda_finger_joint2" value="0.035"/>
<group_state name="open" group="$(arg arm_id)_hand">
<joint name="$(arg arm_id)_finger_joint1" value="0.035"/>
<joint name="$(arg arm_id)_finger_joint2" value="0.035"/>
</group_state>
<group_state name="close" group="hand">
<joint name="panda_finger_joint1" value="0"/>
<joint name="panda_finger_joint2" value="0"/>
<group_state name="close" group="$(arg arm_id)_hand">
<joint name="$(arg arm_id)_finger_joint1" value="0"/>
<joint name="$(arg arm_id)_finger_joint2" value="0"/>
</group_state>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_default_collisions link="panda_hand_sc" />
<enable_collisions link1="panda_hand_sc" link2="panda_link0_sc" />
<enable_collisions link1="panda_hand_sc" link2="panda_link1_sc" />
<enable_collisions link1="panda_hand_sc" link2="panda_link2_sc" />
<enable_collisions link1="panda_hand_sc" link2="panda_link3_sc" />
<!-- Disable collision of hand with all arm links. These are handled by the *_sc links -->
<disable_collisions link1="panda_hand" link2="panda_link0" reason="Never" />
<disable_collisions link1="panda_hand" link2="panda_link1" reason="Never" />
<disable_collisions link1="panda_hand" link2="panda_link2" reason="Never" />
<disable_collisions link1="panda_hand" link2="panda_link3" reason="Never" />
<disable_collisions link1="panda_hand" link2="panda_link4" reason="Never" />
<disable_collisions link1="panda_hand" link2="panda_link5" reason="Never" />
<disable_collisions link1="panda_hand" link2="panda_link6" reason="Never" />
<disable_collisions link1="panda_hand" link2="panda_link7" reason="Never" />
<disable_collisions link1="panda_hand" link2="panda_link8" reason="Never" />
<xacro:finger_collisions finger="panda_leftfinger" />
<xacro:finger_collisions finger="panda_rightfinger" />
<disable_collisions link1="panda_leftfinger" link2="panda_rightfinger" reason="Never" />
<!-- Disable collisions for hand_sc by default (with environment) -->
<disable_default_collisions link="$(arg arm_id)_hand_sc" />
<!-- Reenable collision of hand_sc for selected arm sc links -->
<xacro:property name="link_fmt" value="$(arg arm_id)_link{}_sc" />
<xacro:enable_collisions_for link="$(arg arm_id)_hand_sc" others="${[link_fmt.format(i) for i in [0,1,2,3]]}" />
<!-- Disable collision of hand link with all arm links. These are handled by the *_sc links -->
<xacro:property name="link_fmt" value="$(arg arm_id)_link{}" />
<xacro:disable_collisions_for link="$(arg arm_id)_hand" others="${[link_fmt.format(i) for i in python.range(8)]}" />
<!-- Disable collision of fingers with all arm links -->
<xacro:property name="others" value="${[link_fmt.format(i) for i in python.range(8)] + [xacro.arg('arm_id') + '_hand']}" />
<xacro:disable_collisions_for link="$(arg arm_id)_leftfinger" others="${list(others)}" />
<xacro:disable_collisions_for link="$(arg arm_id)_rightfinger" others="${list(others)}" />
<disable_collisions link1="$(arg arm_id)_leftfinger" link2="$(arg arm_id)_rightfinger" reason="Never" />
</xacro:macro>
</robot>
......@@ -14,47 +14,47 @@ default_acceleration_scaling_factor: 0.1
# max_jerk = (max_acceleration - min_acceleration) / 0.001
joint_limits:
panda_finger_joint1:
$(arg arm_id)_finger_joint1:
has_velocity_limits: true
max_velocity: 0.1
has_acceleration_limits: false
max_acceleration: 0
panda_finger_joint2:
$(arg arm_id)_finger_joint2:
has_velocity_limits: true
max_velocity: 0.1
has_acceleration_limits: false
max_acceleration: 0
panda_joint1:
$(arg arm_id)_joint1:
has_velocity_limits: true
max_velocity: 2.1750
has_acceleration_limits: true
max_acceleration: 3.75
panda_joint2:
$(arg arm_id)_joint2:
has_velocity_limits: true
max_velocity: 2.1750
has_acceleration_limits: true
max_acceleration: 1.875
panda_joint3:
$(arg arm_id)_joint3:
has_velocity_limits: true
max_velocity: 2.1750
has_acceleration_limits: true
max_acceleration: 2.5
panda_joint4:
$(arg arm_id)_joint4:
has_velocity_limits: true
max_velocity: 2.1750
has_acceleration_limits: true
max_acceleration: 3.125
panda_joint5:
$(arg arm_id)_joint5:
has_velocity_limits: true
max_velocity: 2.6100
has_acceleration_limits: true
max_acceleration: 3.75
panda_joint6:
$(arg arm_id)_joint6:
has_velocity_limits: true
max_velocity: 2.6100
has_acceleration_limits: true
max_acceleration: 5
panda_joint7:
$(arg arm_id)_joint7:
has_velocity_limits: true
max_velocity: 2.6100
has_acceleration_limits: true
......
panda_arm: &arm_config
$(arg arm_id)_arm: &arm_config
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.05
manipulator: *arm_config
$(arg arm_id)_manipulator: *arm_config
......@@ -127,7 +127,7 @@ planner_configs:
sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25
dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001
max_failures: 5000 # maximum consecutive failure limit. default: 5000
panda_arm: &arm_config
$(arg arm_id)_arm: &arm_config
planner_configs:
- AnytimePathShortening
- SBL
......@@ -153,10 +153,10 @@ panda_arm: &arm_config
- LazyPRMstar
- SPARS
- SPARStwo
projection_evaluator: joints(panda_joint1,panda_joint2)
projection_evaluator: joints($(arg arm_id)_joint1,$(arg arm_id)_joint2)
longest_valid_segment_fraction: 0.005
manipulator: *arm_config
hand:
$(arg arm_id)_manipulator: *arm_config
$(arg arm_id)_hand:
planner_configs:
- AnytimePathShortening
- SBL
......
......@@ -4,23 +4,24 @@
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
<xacro:arg name="arm_id" default="panda" />
<xacro:include filename="$(find panda_moveit_config)/config/arm.xacro" />
<!-- panda_arm group: eef frame aligned to robot's flanche -->
<xacro:arm name="panda_arm" tip_link="panda_link8"/>
<xacro:arg name="hand" default="false" />
<xacro:arm name="$(arg arm_id)_arm" tip_link="$(arg arm_id)_link8"/>
<!--Add the hand if people request it-->
<xacro:arg name="hand" default="false" />
<xacro:if value="$(arg hand)">
<!-- manipulator group: eef frame aligned to hand -->
<xacro:arm name="manipulator" tip_link="panda_hand_tcp" />
<xacro:arm name="$(arg arm_id)_manipulator" tip_link="$(arg arm_id)_hand_tcp" />
<xacro:include filename="$(find panda_moveit_config)/config/hand.xacro" />
<xacro:hand />
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="hand_tcp" parent_link="panda_hand_tcp" group="hand" parent_group="manipulator" />
<end_effector name="$(arg arm_id)_hand_tcp" parent_link="$(arg arm_id)_hand_tcp" group="$(arg arm_id)_hand" parent_group="$(arg arm_id)_manipulator" />
<!-- old end-effector -->
<end_effector name="hand" parent_link="panda_link8" group="hand" parent_group="panda_arm" />
<end_effector name="$(arg arm_id)_hand" parent_link="$(arg arm_id)_link8" group="$(arg arm_id)_hand" parent_group="$(arg arm_id)_arm" />
</xacro:if>
</robot>
......@@ -9,3 +9,4 @@ sensors:
padding_offset: 0.03
max_update_rate: 1.0
filtered_cloud_topic: filtered_cloud
ns: kinect
......@@ -7,3 +7,4 @@ sensors:
padding_scale: 1.0
max_update_rate: 1.0
filtered_cloud_topic: filtered_cloud
ns: kinect
......@@ -4,16 +4,16 @@ controller_list:
type: FollowJointTrajectory
default: True
joints:
- panda_joint1
- panda_joint2
- panda_joint3
- panda_joint4
- panda_joint5
- panda_joint6
- panda_joint7
- $(arg arm_id)_joint1
- $(arg arm_id)_joint2
- $(arg arm_id)_joint3
- $(arg arm_id)_joint4
- $(arg arm_id)_joint5
- $(arg arm_id)_joint6
- $(arg arm_id)_joint7
- name: franka_gripper
action_ns: gripper_action
type: GripperCommand
default: True
joints:
- panda_finger_joint1
- $(arg arm_id)_finger_joint1
<launch>
<arg name="arm_id" default="panda" />
<!-- specify the planning pipeline -->
<arg name="pipeline" default="ompl" />
......@@ -31,7 +32,7 @@
<arg name="rviz_tutorial" default="false" />
<!-- If needed, broadcast static tf for robot root -->
<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world panda_link0" />
<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world $(arg arm_id)_link0" />
......
<?xml version="1.0"?>
<launch>
<arg name="robot_ip" />
<arg name="load_gripper" />
<!-- Launch real-robot control -->
<include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true" />
<!-- By default use joint position controllers -->
<arg name="transmission" default="position" />
<!-- Start ROS controllers -->
<include file="$(dirname)/ros_controllers.launch" pass_all_args="true" />
<!-- as well as MoveIt demo -->
<include file="$(dirname)/demo.launch" pass_all_args="true">
......
......@@ -24,6 +24,8 @@
<!--Other settings-->
<arg name="load_gripper" default="true" />
<arg name="transmission" default="effort" />
<arg name="arm_id" default="panda" />
<!-- load these non-default MoveGroup capabilities (space seperated) -->
<!--
......@@ -46,6 +48,7 @@
<include file="$(dirname)/planning_context.launch">
<arg name="load_robot_description" value="$(arg load_robot_description)" />
<arg name="load_gripper" value="$(arg load_gripper)" />
<arg name="arm_id" value="$(arg arm_id)" />
</include>
<!-- Planning Pipelines -->
......@@ -54,22 +57,26 @@
<!-- OMPL -->
<include file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="ompl" />
<arg name="arm_id" value="$(arg arm_id)" />
</include>
<!-- CHOMP -->
<include file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="chomp" />
<arg name="arm_id" value="$(arg arm_id)" />
</include>
<!-- Pilz Industrial Motion -->
<include file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="pilz_industrial_motion_planner" />
<arg name="arm_id" value="$(arg arm_id)" />
</include>
<!-- Support custom planning pipeline -->
<include if="$(eval arg('pipeline') not in ['ompl', 'chomp', 'pilz_industrial_motion_planner'])"
file="$(dirname)/planning_pipeline.launch.xml">
<arg name="pipeline" value="$(arg pipeline)" />
<arg name="arm_id" value="$(arg arm_id)" />
</include>
</group>
......
......@@ -5,9 +5,8 @@ Panels:
Property Tree Widget:
Expanded:
- /MotionPlanning1
- /Tag Detection Camera1
Splitter Ratio: 0.5
Tree Height: 357
Tree Height: 226
- Class: rviz/Help
Name: Help
- Class: rviz/Views
......@@ -15,17 +14,6 @@ Panels:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Displays
Help Height: 70
Name: Displays
Property Tree Widget:
Expanded: ~
Splitter Ratio: 0.5
Tree Height: 371
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
......@@ -35,7 +23,7 @@ Visualization Manager:
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
......@@ -47,126 +35,31 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Acceleration_Scaling_Factor: 0.1
Class: moveit_rviz_plugin/MotionPlanning
- Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
Move Group Namespace: ""
MoveIt_Allow_Approximate_IK: false
MoveIt_Allow_External_Program: false
MoveIt_Allow_Replanning: false
MoveIt_Allow_Sensor_Positioning: false
MoveIt_Planning_Attempts: 10
MoveIt_Planning_Time: 5
MoveIt_Use_Cartesian_Path: false
MoveIt_Use_Constraint_Aware_IK: false
MoveIt_Workspace:
Center:
X: 0
Y: 0
Z: 0
Size:
X: 2
Y: 2
Z: 2
Name: MotionPlanning
Planned Path:
Color Enabled: false
Interrupt Display: false
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
panda_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_hand_tcp:
Alpha: 1
Show Axes: false
Show Trail: false
panda_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link8:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Loop Animation: false
Links: ~
Loop Animation: true
Robot Alpha: 0.5
Robot Color: 150; 50; 150
Show Robot Collision: false
Show Robot Visual: false
Show Robot Visual: true
Show Trail: false
State Display Time: 0.05 s
Trail Step Size: 1
Trajectory Topic: move_group/display_planned_path
Use Sim Time: false
Planning Metrics:
Payload: 1
Show Joint Torques: false
Show Manipulability: false
Show Manipulability Index: false
Show Weight Limit: false
TextHeight: 0.07999999821186066
Planning Request:
Colliding Link Color: 255; 0; 0
Goal State Alpha: 1
Goal State Color: 250; 128; 0
Interactive Marker Size: 0
Joint Violation Color: 255; 0; 255
Planning Group: hand
Query Goal State: false
Query Goal State: true
Query Start State: false
Show Workspace: false
Start State Alpha: 1
......@@ -175,306 +68,19 @@ Visualization Manager:
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 1
Scene Color: 50; 230; 50
Scene Display Time: 0.009999999776482582
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
panda_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_hand_tcp:
Alpha: 1
Show Axes: false
Show Trail: false
panda_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link8:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Value: true
Velocity_Scaling_Factor: 0.1
- Class: rviz/Camera
Enabled: false
Image Rendering: background and overlay
Image Topic: ""
Name: Camera
Overlay Alpha: 0.5
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: false
Visibility:
Camera Image: true
Grid: true
MotionPlanning: true
PlanningScene: true
RobotModel: true
Tag Detection Camera: true
Value: true
Zoom Factor: 1
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
panda_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_hand_tcp:
Alpha: 1
Show Axes: false
Show Trail: false
panda_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link8:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Class: moveit_rviz_plugin/PlanningScene
Enabled: true
Move Group Namespace: ""
Name: PlanningScene
Planning Scene Topic: move_group/monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 0.8999999761581421
Scene Color: 50; 230; 50
Scene Display Time: 0.009999999776482582
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
panda_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_hand_tcp:
Alpha: 1
Show Axes: false
Show Trail: false
panda_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link8:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /tag_detections_image
Max Value: 1
Median window: 5
Min Value: 0
Name: Tag Detection Camera
Normalize Range: true
Queue Size: 2
Transport Hint: compressed
Unreliable: false
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /usb_cam/image_color
Max Value: 1
Median window: 5
Min Value: 0
Name: Camera Image
Normalize Range: true
Queue Size: 2
Transport Hint: compressed
Unreliable: false
Links: ~
Robot Alpha: 0.5
Show Scene Robot: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: panda_link0
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
......@@ -485,34 +91,30 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 1.9689866304397583
Distance: 2.0
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.75
Focal Point:
X: -0.03587188199162483
Y: 0.11581680178642273
Z: 0.06132292002439499
X: -0.1
Y: 0.25
Z: 0.30
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Focal Shape Size: 0.05
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.5799999833106995
Near Clip Distance: 0.01
Pitch: 0.5
Target Frame: panda_link0
Yaw: 0.11676633358001709
Yaw: -0.6232355833053589
Saved: ~
Window Geometry:
Camera:
collapsed: false
Camera Image:
collapsed: false
Displays:
collapsed: false
Height: 1043
Height: 848
Help:
collapsed: false
Hide Left Dock: false
......@@ -521,11 +123,9 @@ Window Geometry:
collapsed: false
MotionPlanning - Trajectory Slider:
collapsed: false
QMainWindow State: 000000ff00000000fd000000020000000000000310000003b9fc020000000afb000000100044006900730070006c006100790073000000003d000003b9000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000001600fffffffb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000010c000001ce0000017d00fffffffb000000100044006900730070006c00610079007300000002cb0000012b000000c900fffffffb0000000c00430061006d006500720061000000028f000001670000000000000000fb0000000a0049006d00610067006500000001780000008400000000000000000000000100000303000003b9fc0200000002fb0000001800430061006d00650072006100200049006d006100670065010000003d000001d10000001600fffffffb0000002800540061006700200044006500740065006300740069006f006e002000430061006d0065007200610100000214000001e20000001600ffffff00000477000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Tag Detection Camera:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Views:
collapsed: false
Width: 1920
X: 2560
Y: 360
Width: 1291
X: 454
Y: 25
......@@ -12,6 +12,7 @@
<arg name="start_state_max_bounds_error" default="0.1" />
<arg name="jiggle_fraction" default="0.05" />
<arg name="arm_id" default="panda" />
<param name="planning_plugin" value="ompl_interface/OMPLPlanner" />
<param name="request_adapters" value="$(arg planning_adapters)" />
......@@ -21,6 +22,6 @@
<!-- Add MoveGroup capabilities specific to this pipeline -->
<!-- <param name="capabilities" value="" /> -->
<rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/>
<rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml" subst_value="true"/>
</launch>
......@@ -2,25 +2,26 @@
<!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
<arg name="load_gripper" default="true" />
<arg name="load_robot_description" default="false"/>
<arg name="arm_id" default="panda" />
<!-- The name of the parameter under which the URDF is loaded -->
<arg name="robot_description" default="robot_description"/>
<!-- Load universal robot description format (URDF) -->
<param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro hand:=$(arg load_gripper) '$(find franka_description)/robots/panda_arm.urdf.xacro'"/>
<param name="$(arg robot_description)" command="xacro '$(find franka_description)/robots/panda/panda.urdf.xacro' hand:=$(arg load_gripper) arm_id:=$(arg arm_id)" if="$(arg load_robot_description)" />
<!-- The semantic description that corresponds to the URDF -->
<param name="$(arg robot_description)_semantic" command="$(find xacro)/xacro '$(find panda_moveit_config)/config/panda.srdf.xacro' hand:=$(arg load_gripper)"/>
<param name="$(arg robot_description)_semantic" command="xacro '$(find panda_moveit_config)/config/panda.srdf.xacro' hand:=$(arg load_gripper) arm_id:=$(arg arm_id)" />
<!-- Load updated joint limits (override information from URDF) -->
<group ns="$(arg robot_description)_planning">
<rosparam command="load" file="$(find panda_moveit_config)/config/joint_limits.yaml"/>
<rosparam command="load" file="$(find panda_moveit_config)/config/cartesian_limits.yaml"/>
<rosparam command="load" file="$(find panda_moveit_config)/config/joint_limits.yaml" subst_value="true" />
<rosparam command="load" file="$(find panda_moveit_config)/config/cartesian_limits.yaml" subst_value="true"/>
</group>
<!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
<group ns="$(arg robot_description)_kinematics">
<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
<rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml" subst_value="true"/>
</group>
......
......@@ -4,7 +4,8 @@
It is assumed that all planning pipelines are named XXX_planning_pipeline.launch -->
<arg name="pipeline" default="ompl" />
<arg name="arm_id" default="panda" />
<include ns="$(arg pipeline)" file="$(dirname)/$(arg pipeline)_planning_pipeline.launch.xml" />
<include ns="$(arg pipeline)" file="$(dirname)/$(arg pipeline)_planning_pipeline.launch.xml" pass_all_args="true"/>
</launch>
......@@ -2,9 +2,9 @@
<launch>
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find panda_moveit_config)/config/ros_controllers.yaml" command="load"/>
<rosparam file="$(find panda_moveit_config)/config/ros_controllers.yaml" command="load" subst_value="true"/>
<!-- Load and start the controllers -->
<node ns="/" name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
args="$(arg transmission)_joint_trajectory_controller" />
</launch>
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment