Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

redo add arm_id to panda_moveit_config #106

Merged
merged 2 commits into from
Mar 22, 2022
Merged

redo add arm_id to panda_moveit_config #106

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f59ddfb
redo add arm_id to panda_moveit_config
Mar 21, 2022
811509c
Replace remaining names
rhaschke Mar 22, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
96 changes: 48 additions & 48 deletions config/arm.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link0" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link1" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link2" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link3" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link4" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link5" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link6" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link7" reason="Never" />
<disable_collisions link1="${link}" link2="$(arg arm_id)_link8" reason="Never" />
</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:collision link="$(arg arm_id)_link0"/>
<enable_collisions link1="$(arg arm_id)_link0_sc" link2="$(arg arm_id)_link5_sc" />
<enable_collisions link1="$(arg arm_id)_link0_sc" link2="$(arg arm_id)_link6_sc" />
<enable_collisions link1="$(arg arm_id)_link0_sc" link2="$(arg arm_id)_link7_sc" />
<enable_collisions link1="$(arg arm_id)_link0_sc" link2="$(arg arm_id)_link8_sc" />
<xacro:collision link="$(arg arm_id)_link1" />
<enable_collisions link1="$(arg arm_id)_link1_sc" link2="$(arg arm_id)_link5_sc" />
<enable_collisions link1="$(arg arm_id)_link1_sc" link2="$(arg arm_id)_link6_sc" />
<enable_collisions link1="$(arg arm_id)_link1_sc" link2="$(arg arm_id)_link7_sc" />
<enable_collisions link1="$(arg arm_id)_link1_sc" link2="$(arg arm_id)_link8_sc" />
<xacro:collision link="$(arg arm_id)_link2" />
<enable_collisions link1="$(arg arm_id)_link2_sc" link2="$(arg arm_id)_link5_sc" />
<enable_collisions link1="$(arg arm_id)_link2_sc" link2="$(arg arm_id)_link6_sc" />
<enable_collisions link1="$(arg arm_id)_link2_sc" link2="$(arg arm_id)_link7_sc" />
<enable_collisions link1="$(arg arm_id)_link2_sc" link2="$(arg arm_id)_link8_sc" />
<xacro:collision link="$(arg arm_id)_link3" />
<enable_collisions link1="$(arg arm_id)_link3_sc" link2="$(arg arm_id)_link7_sc" />
<enable_collisions link1="$(arg arm_id)_link3_sc" link2="$(arg arm_id)_link8_sc" />
<xacro:collision link="$(arg arm_id)_link4" />
<xacro:collision link="$(arg arm_id)_link5" />
<xacro:collision link="$(arg arm_id)_link6" />
<xacro:collision link="$(arg arm_id)_link7" />
<xacro:collision link="$(arg arm_id)_link8" />
</robot>
35 changes: 13 additions & 22 deletions config/fake_controllers.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,31 +1,22 @@
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
68 changes: 34 additions & 34 deletions config/hand.xacro
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,16 +3,16 @@
<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" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link0" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link1" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link2" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link3" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link4" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link5" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link6" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link7" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_link8" reason="Never" />
<disable_collisions link1="${finger}" link2="$(arg arm_id)_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-->
Expand All @@ -21,41 +21,41 @@
<!--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"/>
<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"/>
<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"/>
<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_default_collisions link="$(arg arm_id)_hand_sc" />
<enable_collisions link1="$(arg arm_id)_hand_sc" link2="$(arg arm_id)_link0_sc" />
<enable_collisions link1="$(arg arm_id)_hand_sc" link2="$(arg arm_id)_link1_sc" />
<enable_collisions link1="$(arg arm_id)_hand_sc" link2="$(arg arm_id)_link2_sc" />
<enable_collisions link1="$(arg arm_id)_hand_sc" link2="$(arg arm_id)_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" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link0" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link1" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link2" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link3" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link4" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link5" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link6" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_link7" reason="Never" />
<disable_collisions link1="$(arg arm_id)_hand" link2="$(arg arm_id)_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" />
<xacro:finger_collisions finger="$(arg arm_id)_leftfinger" />
<xacro:finger_collisions finger="$(arg arm_id)_rightfinger" />
<disable_collisions link1="$(arg arm_id)_leftfinger" link2="$(arg arm_id)_rightfinger" reason="Never" />
</xacro:macro>
</robot>
18 changes: 9 additions & 9 deletions config/joint_limits.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion config/kinematics.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
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
Expand Down
4 changes: 2 additions & 2 deletions config/ompl_planning.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -153,7 +153,7 @@ 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:
Expand Down
Loading