From 559a12bf178a0fa8581ebe6e26778547c2bd2510 Mon Sep 17 00:00:00 2001 From: Richard Bormann Date: Tue, 7 Nov 2017 10:18:04 +0100 Subject: [PATCH 1/5] changed group name/joints for raw3-1 --- .../robots/raw3-1/moveit/config/raw3-1.srdf | 232 +++++++++--------- 1 file changed, 117 insertions(+), 115 deletions(-) diff --git a/cob_moveit_config/robots/raw3-1/moveit/config/raw3-1.srdf b/cob_moveit_config/robots/raw3-1/moveit/config/raw3-1.srdf index a042b50e9..27685bdc1 100644 --- a/cob_moveit_config/robots/raw3-1/moveit/config/raw3-1.srdf +++ b/cob_moveit_config/robots/raw3-1/moveit/config/raw3-1.srdf @@ -10,15 +10,30 @@ - - - - + + + + + + + + - + - + + + + + + + + + + @@ -36,29 +51,32 @@ - - - - - + + + + + - - - - + + + - + + + + + @@ -75,36 +93,67 @@ - - - - - + + + + + + + + + + + + + + + + + + + + + + - + + + + - + + + - + + + + - + + + + - + + + + @@ -117,12 +166,9 @@ - - - - - + + @@ -135,12 +181,9 @@ - - - - - + + @@ -152,12 +195,9 @@ - - - - - + + @@ -168,12 +208,9 @@ - - - - - + + @@ -183,12 +220,9 @@ - - - - - + + @@ -197,12 +231,10 @@ - - - + - - + + @@ -211,96 +243,66 @@ - - - - - + + - - - - - + + - - - - - - + + - - - + + - - + + - - - + - - + + - - - - - + + - - - - - + + - - - - - - - - - - - - - - - - - - - - + + + + + + + + + - - - + + + From d3a679221a3399bdea62226f03ed61d6a31ffc11 Mon Sep 17 00:00:00 2001 From: Richard Bormann Date: Mon, 20 Nov 2017 15:57:31 +0100 Subject: [PATCH 2/5] updated homing digital input for wheel --- cob_hardware_config/robots/raw3-1/config/base_driver.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cob_hardware_config/robots/raw3-1/config/base_driver.yaml b/cob_hardware_config/robots/raw3-1/config/base_driver.yaml index c0fa9d827..acacfed5f 100644 --- a/cob_hardware_config/robots/raw3-1/config/base_driver.yaml +++ b/cob_hardware_config/robots/raw3-1/config/base_driver.yaml @@ -8,7 +8,7 @@ nodes: ##Wheel 1 fl_caster_rotation_joint: motor_layer: - homing_event: 19 # -> digin + homing_event: 17 # -> digin homing_speed: -5093 # -1.0 rs/s homing_offset: -12880 fl_caster_r_wheel_joint: From 1b5bd062cab1da6da0e53147a503c32ba4bad08a Mon Sep 17 00:00:00 2001 From: Richard Bormann Date: Fri, 2 Feb 2018 14:09:06 +0100 Subject: [PATCH 3/5] add joint constraints --- .../moveit/config/constraints_limit.yaml | 90 +++++++++++++++++++ 1 file changed, 90 insertions(+) create mode 100644 cob_moveit_config/robots/raw3-1/moveit/config/constraints_limit.yaml diff --git a/cob_moveit_config/robots/raw3-1/moveit/config/constraints_limit.yaml b/cob_moveit_config/robots/raw3-1/moveit/config/constraints_limit.yaml new file mode 100644 index 000000000..fac778f8b --- /dev/null +++ b/cob_moveit_config/robots/raw3-1/moveit/config/constraints_limit.yaml @@ -0,0 +1,90 @@ + +################################## +#joint constraint +################################## + +joint_constraint_limits: +# arm_elbow_joint: +# ceneter_position: 0 +# tolerance_above: 0 +# tolerance_below: 0 +# arm_shoulder_lift_joint: +# ceneter_position: 0 +# tolerance_above: 0 +# tolerance_below: 0 + arm_shoulder_pan_joint: + ceneter_position: -0.675 + tolerance_above: 0.830 + tolerance_below: 1.05 + arm_wrist_1_joint: + ceneter_position: -3.14 + tolerance_above: 1.57 + tolerance_below: 3.14 + arm_wrist_2_joint: + ceneter_position: -1.5 + tolerance_above: 1.03 + tolerance_below: 1.15 +# arm_wrist_3_joint: +# ceneter_position: 0 +# tolerance_above: 0 +# tolerance_below: 0 + +################################## +#position constraint --> check what required +################################## + +#position_constraint_limits: +# arm_elbow_joint: +# ceneter_position: 0 +# tolerance_above: 0 +# tolerance_below: 0 +# arm_shoulder_lift_joint: +# ceneter_position: 0 +# tolerance_above: 0 +# tolerance_below: 0 +# arm_shoulder_pan_joint: +# ceneter_position: 0 +# tolerance_above: 0 +# tolerance_below: 0 +# arm_wrist_1_joint: +# ceneter_position: 0 +# tolerance_above: 0 +# tolerance_below: 0 +# arm_wrist_2_joint: +# ceneter_position: -1.5 +# tolerance_above: 0.7652 +# tolerance_below: 1.15 +# arm_wrist_3_joint: +# ceneter_position: 0 +# tolerance_above: 0 +# tolerance_below: 0 + +################################## +#orientation constraint --> check what required +################################## + +#joint_constraint_limits: +# arm_elbow_joint: +# ceneter_position: -0.675 +# tolerance_above: 0.785 +# tolerance_below: 1.05 +# arm_shoulder_lift_joint: +# ceneter_position: 0 +# tolerance_above: 0 +# tolerance_below: 0 +# arm_shoulder_pan_joint: +# ceneter_position: 0 +# tolerance_above: 0 +# tolerance_below: 0 +# arm_wrist_1_joint: +# ceneter_position: -3.14 +# tolerance_above: 1.57 +# tolerance_below: 3.14 +# arm_wrist_2_joint: +# ceneter_position: -1.5 +# tolerance_above: 0.7652 +# tolerance_below: 1.15 +# arm_wrist_3_joint: +# ceneter_position: 0 +# tolerance_above: 0 +# tolerance_below: 0 From 3fc34f7008ffe14ec44c6f3fd454b546d82f38b7 Mon Sep 17 00:00:00 2001 From: Richard Bormann Date: Fri, 2 Feb 2018 14:09:31 +0100 Subject: [PATCH 4/5] stomp configuration file --- .../raw3-1/moveit/config/stomp_config.yaml | 38 +++++++++++++++++++ .../raw3-1/moveit/config/stomp_planning.yaml | 0 2 files changed, 38 insertions(+) create mode 100644 cob_moveit_config/robots/raw3-1/moveit/config/stomp_config.yaml create mode 100644 cob_moveit_config/robots/raw3-1/moveit/config/stomp_planning.yaml diff --git a/cob_moveit_config/robots/raw3-1/moveit/config/stomp_config.yaml b/cob_moveit_config/robots/raw3-1/moveit/config/stomp_config.yaml new file mode 100644 index 000000000..5d910c4c7 --- /dev/null +++ b/cob_moveit_config/robots/raw3-1/moveit/config/stomp_config.yaml @@ -0,0 +1,38 @@ +stomp/arm: + group_name: arm + optimization: + num_timesteps: 20 + num_iterations: 50 + num_iterations_after_valid: 0 + num_rollouts: 10 + max_rollouts: 100 + 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.3, 1.0, 0.5, 0.4, 0.3, 0.3] + cost_functions: + - class: stomp_moveit/ObstacleDistanceGradient + max_distance: 0.2 + cost_weight: 1.0 + 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: [0, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/ControlCostProjectionMatrix + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.02 + rgb: [191, 0, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized + diff --git a/cob_moveit_config/robots/raw3-1/moveit/config/stomp_planning.yaml b/cob_moveit_config/robots/raw3-1/moveit/config/stomp_planning.yaml new file mode 100644 index 000000000..e69de29bb From a7d5c7c11c138666f66b746af8eff17e3ea26064 Mon Sep 17 00:00:00 2001 From: Richard Bormann Date: Fri, 2 Feb 2018 14:15:54 +0100 Subject: [PATCH 5/5] add stomp planning --- .../raw3-1/moveit/config/stomp_planning.yaml | 49 +++++++++++++++++++ 1 file changed, 49 insertions(+) diff --git a/cob_moveit_config/robots/raw3-1/moveit/config/stomp_planning.yaml b/cob_moveit_config/robots/raw3-1/moveit/config/stomp_planning.yaml index e69de29bb..b8c96e28a 100644 --- a/cob_moveit_config/robots/raw3-1/moveit/config/stomp_planning.yaml +++ b/cob_moveit_config/robots/raw3-1/moveit/config/stomp_planning.yaml @@ -0,0 +1,49 @@ +# Optimization Task +num_feature_basis_functions: 1 +trajectory_duration: 8.0 +num_time_steps: 30 +publish_trajectory_markers: False +publish_best_trajectory_marker: True + + +# STOMP +max_iterations: 10 +max_iterations_after_collision_free: 1 +max_rollouts: 100 +min_rollouts: 30 +num_rollouts_per_iteration: 10 +start_state_max_bounds_error: 1.0 +start_state_max_dt: 2.0 + +# STOMP (optional) +use_noise_adaptation: true +write_to_file: false +use_openmp: false + +noise_coefficients: + - group_name: arm + stddev: [0.3, 1.0, 1.0, 0.8, 0.3, 0.3, 0.3] + min_stddev: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + decay: [0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8] + - group_name: robot + stddev: [1.2, 1.0, 1.0, 0.8, 0.3, 0.3] + min_stddev: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + decay: [0.8, 0.8, 0.8, 0.8, 0.8, 0.8] + +features: +- class: stomp_moveit_interface/ObstacleAvoidanceFeature + collision_clearance: 0.02 +#- class: stomp_moveit_interface/CollisionFeature +# report_validity: false # report state validity based on distance field +# collision_clearance: 0.02 +# debug_collisions: false +# collision_space: +# size_x: 8.5 # The X dimension in meters of the volume to represent +# size_y: 6.0 # The Y dimension in meters of the volume to represent +# size_z: 4.0 # The Z dimension in meters of the volume to represent +# origin_x: 0.0 +# origin_y: 0.0 +# origin_z: 1.5 +# resolution: 0.03 +# collision_tolerance: 0.01 +# max_propagation_distance: 0.1