Commit 57764d7b authored by Johannes Mey's avatar Johannes Mey
Browse files

update according to upstream

parent f2877be7
......@@ -35,26 +35,17 @@
<xacro:macro name="collision" params="link">
<!-- Enable (environmental) collisions of ${link}_sc -->
<collision_default link="${link}_sc" allow="ALWAYS" />
<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="Default" />
<disable_collisions link1="${link}" link2="panda_link1" reason="Default" />
<disable_collisions link1="${link}" link2="panda_link2" reason="Default" />
<disable_collisions link1="${link}" link2="panda_link3" reason="Default" />
<disable_collisions link1="${link}" link2="panda_link4" reason="Default" />
<disable_collisions link1="${link}" link2="panda_link5" reason="Default" />
<disable_collisions link1="${link}" link2="panda_link6" reason="Default" />
<disable_collisions link1="${link}" link2="panda_link7" reason="Default" />
<disable_collisions link1="${link}" link2="panda_link8" reason="Default" />
<disable_collisions link1="${link}_sc" link2="panda_link0" reason="Default" />
<disable_collisions link1="${link}_sc" link2="panda_link1" reason="Default" />
<disable_collisions link1="${link}_sc" link2="panda_link2" reason="Default" />
<disable_collisions link1="${link}_sc" link2="panda_link3" reason="Default" />
<disable_collisions link1="${link}_sc" link2="panda_link4" reason="Default" />
<disable_collisions link1="${link}_sc" link2="panda_link5" reason="Default" />
<disable_collisions link1="${link}_sc" link2="panda_link6" reason="Default" />
<disable_collisions link1="${link}_sc" link2="panda_link7" reason="Default" />
<disable_collisions link1="${link}_sc" link2="panda_link8" reason="Default" />
<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>
<xacro:collision link="panda_link0"/>
<enable_collisions link1="panda_link0_sc" link2="panda_link5_sc" />
......
......@@ -3,17 +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="Default" />
<disable_collisions link1="${finger}" link2="panda_link1" reason="Default" />
<disable_collisions link1="${finger}" link2="panda_link2" reason="Default" />
<disable_collisions link1="${finger}" link2="panda_link3" reason="Default" />
<disable_collisions link1="${finger}" link2="panda_link4" reason="Default" />
<disable_collisions link1="${finger}" link2="panda_link5" reason="Default" />
<disable_collisions link1="${finger}" link2="panda_link6" reason="Default" />
<disable_collisions link1="${finger}" link2="panda_link7" reason="Default" />
<disable_collisions link1="${finger}" link2="panda_link8" reason="Default" />
<disable_collisions link1="${finger}" link2="panda_hand" reason="Default" />
<disable_collisions link1="${finger}" link2="panda_hand_sc" reason="Default" />
<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-->
......@@ -39,24 +38,24 @@
<!--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. -->
<collision_default link="panda_hand_sc" allow="ALWAYS" />
<enable_collisions link1="panda_hand_sc" link2="panda_link0" />
<enable_collisions link1="panda_hand_sc" link2="panda_link1" />
<enable_collisions link1="panda_hand_sc" link2="panda_link2" />
<enable_collisions link1="panda_hand_sc" link2="panda_link3" />
<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" />
<disable_collisions link1="panda_hand" link2="panda_link1" />
<disable_collisions link1="panda_hand" link2="panda_link2" />
<disable_collisions link1="panda_hand" link2="panda_link3" />
<disable_collisions link1="panda_hand" link2="panda_link4" />
<disable_collisions link1="panda_hand" link2="panda_link5" />
<disable_collisions link1="panda_hand" link2="panda_link6" />
<disable_collisions link1="panda_hand" link2="panda_link7" />
<disable_collisions link1="panda_hand" link2="panda_link8" />
<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="Default" />
<disable_collisions link1="panda_leftfinger" link2="panda_rightfinger" reason="Never" />
</xacro:macro>
</robot>
panda_arm:
panda_arm: &arm_config
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.05
\ No newline at end of file
kinematics_solver_timeout: 0.05
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:
panda_arm: &arm_config
planner_configs:
- AnytimePathShortening
- SBL
......@@ -155,6 +155,7 @@ panda_arm:
- SPARStwo
projection_evaluator: joints(panda_joint1,panda_joint2)
longest_valid_segment_fraction: 0.005
manipulator: *arm_config
hand:
planner_configs:
- AnytimePathShortening
......@@ -181,31 +182,3 @@ hand:
- LazyPRMstar
- SPARS
- SPARStwo
panda_arm_hand:
planner_configs:
- AnytimePathShortening
- SBL
- EST
- LBKPIECE
- BKPIECE
- KPIECE
- RRT
- RRTConnect
- RRTstar
- TRRT
- PRM
- PRMstar
- FMT
- BFMT
- PDST
- STRIDE
- BiTRRT
- LBTRRT
- BiEST
- ProjEST
- LazyPRM
- LazyPRMstar
- SPARS
- SPARStwo
projection_evaluator: joints(panda_joint1,panda_joint2)
longest_valid_segment_fraction: 0.005
......@@ -5,14 +5,15 @@
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
<xacro:include filename="$(find panda_moveit_config)/config/arm.xacro" />
<xacro:arm name="manipulator" tip_link="panda_hand_tcp" />
<!-- old group name with old end-effector link -->
<!-- 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" />
<!--Add the hand if people request it-->
<xacro:if value="$(arg hand)">
<!-- manipulator group: eef frame aligned to hand -->
<xacro:arm name="manipulator" tip_link="panda_hand_tcp" />
<xacro:include filename="$(find panda_moveit_config)/config/hand.xacro" />
<xacro:hand />
......
stomp/panda_arm:
group_name: panda_arm
optimization:
num_timesteps: 60
num_iterations: 40
num_iterations_after_valid: 0
num_rollouts: 30
max_rollouts: 30
initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]
control_cost_weight: 0.0
task:
noise_generator:
- class: stomp_moveit/NormalDistributionSampling
stddev: [0.05, 0.8, 1.0, 0.8, 0.4, 0.4, 0.4]
cost_functions:
- class: stomp_moveit/CollisionCheck
collision_penalty: 1.0
cost_weight: 1.0
kernel_window_percentage: 0.2
longest_valid_joint_move: 0.05
noisy_filters:
- class: stomp_moveit/JointLimits
lock_start: True
lock_goal: True
- class: stomp_moveit/MultiTrajectoryVisualization
line_width: 0.02
rgb: [255, 255, 0]
marker_array_topic: stomp_trajectories
marker_namespace: noisy
update_filters:
- class: stomp_moveit/PolynomialSmoother
poly_order: 6
- class: stomp_moveit/TrajectoryVisualization
line_width: 0.05
rgb: [0, 191, 255]
error_rgb: [255, 0, 0]
publish_intermediate: True
marker_topic: stomp_trajectory
marker_namespace: optimized
\ No newline at end of file
optimization:
num_timesteps: 60
num_iterations: 40
num_iterations_after_valid: 0
num_rollouts: 30
max_rollouts: 30
initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST]
control_cost_weight: 0.0
task:
noise_generator:
- class: stomp_moveit/NormalDistributionSampling
stddev: [0.05, 0.8, 1.0, 0.8, 0.4, 0.4, 0.4]
cost_functions:
- class: stomp_moveit/CollisionCheck
collision_penalty: 1.0
cost_weight: 1.0
kernel_window_percentage: 0.2
longest_valid_joint_move: 0.05
noisy_filters:
- class: stomp_moveit/JointLimits
lock_start: True
lock_goal: True
- class: stomp_moveit/MultiTrajectoryVisualization
line_width: 0.02
rgb: [255, 255, 0]
marker_array_topic: stomp_trajectories
marker_namespace: noisy
update_filters:
- class: stomp_moveit/PolynomialSmoother
poly_order: 6
- class: stomp_moveit/TrajectoryVisualization
line_width: 0.05
rgb: [0, 191, 255]
error_rgb: [255, 0, 0]
publish_intermediate: True
marker_topic: stomp_trajectory
marker_namespace: optimized
......@@ -6,5 +6,5 @@
<!-- Load and start the controllers -->
<node ns="/" name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
args="$(arg transmission)_joint_trajectory_controller franka_gripper" />
args="$(arg transmission)_joint_trajectory_controller" />
</launch>
......@@ -21,5 +21,14 @@
<!-- Add MoveGroup capabilities specific to this pipeline -->
<!-- <param name="capabilities" value="" /> -->
<rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml"/>
<!-- Load parameters for both groups, panda_arm and manipulator -->
<group ns="panda_arm">
<rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml" />
<param name="group_name" value="panda_arm" />
</group>
<group ns="manipulator">
<rosparam command="load" file="$(find panda_moveit_config)/config/stomp_planning.yaml" />
<param name="group_name" value="manipulator" />
</group>
</launch>
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment