add new task parameters

CMakeLists.txt

@@ -5,6 +5,6 @@ find_package(catkin REQUIRED)
 
 catkin_package()
 
-install(DIRECTORY pick_and_place
+install(DIRECTORY pick_and_place reaching_goal
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )

pick_and_place/mpc_params.yaml

@@ -2,20 +2,21 @@ ocp:
     dt: 0.01
     horizon_size: 20
     armature: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
-    gripper_weight: 500
-    state_weight: 5
-    control_weight: 0.001
+    # settings 1 g_w 0.5 s_w 12.0 c_w 0.0008  hs 20
+    gripper_weight: 0.5
+    state_weight: 12.0
+    control_weight: 0.0008
     max_qp_iter: 100
-    max_iter: 7
+    max_iter: 3
     use_constraints: True
     activate_callback: False
     effector_frame_name: "panda_hand_tcp"
+    increasing_weights:
+        max: 100.0
+        percent: 0.9
+        time_reach_percent: 6
 save_predictions_and_refs: True
 rate: 100
 use_vision: False
 use_vision_simulated: True
 start_visual_servoing_dist: 0.08
-increasing_weights:
-    max: 100
-    percent: 0.9
-    time_reach_percent: 6

pick_and_place/obstacle_params.yaml

@@ -1,20 +1,32 @@
 obstacle1:
-    type: "sphere"
+    type: "capsule"
     radius: 0.1
-    translation: [0.6,0.09,0.32]  # tx,ty,tz  
-    rotation: [0.0, 0.0, 0.0, 1] #qx,qy,qz,qw 
+    halfLength: 0.2
+    translation: [0.835, 0, 0.43]
+    rotation: [0.5, 0.5, 0.5, -0.5]
+
+obstacle2:
+    type: "capsule"
+    radius: 0.1
+    halfLength: 0.2
+    translation: [0.32, 0, 0.15]
+    rotation: [0.5, 0.5, 0.5, -0.5]
+
+obstacle3:
+    type: "capsule"
+    radius: 0.1
+    halfLength: 0.2
+    translation: [0.6, -0.5, 0.15]
+    rotation: [0.0, 7.07106781e-1, 0.0, 7.07106781e-1]
 
 support_link_0:
     in_hpp: True
     type: "box"
     x: 1.703
     y: 0.803
     z: 0.76
-    translation: [0, 0, -0.38]  # tx,ty,tz
+    translation: [0, 0, -0.38] # tx,ty,tz
     rotation: [0, 0, 0, 1] #qx,qy,qz,qw
 
-collision_pairs: 
-    - ["panda_leftfinger_0", "obstacle1"]
-    - ["panda_rightfinger_0", "obstacle1"]
-
-
+collision_pairs:
+    - ["panda_leftfinger_capsule_0", "obstacle1"]

pick_and_place/obstacle_params_config_1.yaml

@@ -0,0 +1,40 @@
+obstacle1:
+    type: "capsule"
+    radius: 0.1
+    halfLength: 0.2
+    translation: [0.81, 0, 0.45]
+    rotation: [0.5, 0.5, 0.5, -0.5]
+
+obstacle2:
+    type: "capsule"
+    radius: 0.1
+    halfLength: 0.2
+    translation: [0.32, 0, 0.15]
+    rotation: [0.5, 0.5, 0.5, -0.5]
+
+obstacle3:
+    type: "capsule"
+    radius: 0.1
+    halfLength: 0.2
+    translation: [0.6, -0.5, 0.15]
+    rotation: [0.0, 7.07106781e-1, 0.0, 7.07106781e-1]
+
+support_link_0:
+    in_hpp: True
+    type: "box"
+    x: 1.703
+    y: 0.803
+    z: 0.76
+    translation: [0, 0, -0.38] # tx,ty,tz
+    rotation: [0, 0, 0, 1] #qx,qy,qz,qw
+
+collision_pairs:
+    - ["panda_link5_sc_capsule_0", "obstacle1"]
+    - ["panda_link7_sc_0", "obstacle1"]
+    - ["panda_leftfinger_capsule_0", "obstacle1"]
+    - ["panda_link3_sc_capsule_0", "obstacle2"]
+    - ["panda_link5_sc_capsule_0", "obstacle2"]
+    - ["panda_leftfinger_capsule_0", "obstacle2"]
+    - ["panda_link3_sc_capsule_0", "obstacle3"]
+    - ["panda_link5_sc_capsule_0", "obstacle3"]
+    - ["panda_leftfinger_capsule_0", "obstacle3"]

pick_and_place/obstacle_params_config_2.yaml

@@ -0,0 +1,40 @@
+obstacle1:
+    type: "capsule"
+    radius: 0.1
+    halfLength: 0.2
+    translation: [0.7, -0.18, 0.45]
+    rotation: [0.0, 7.07106781e-1, 0.0, 7.07106781e-1]
+
+obstacle2:
+    type: "capsule"
+    radius: 0.1
+    halfLength: 0.2
+    translation: [0.36, -0.1, 0.15]
+    rotation: [0.5, 0.5, 0.5, -0.5]
+
+obstacle3:
+    type: "capsule"
+    radius: 0.1
+    halfLength: 0.2
+    translation: [0.6, -0.27, 0.15]
+    rotation: [0.0, 7.07106781e-1, 0.0, 7.07106781e-1]
+
+support_link_0:
+    in_hpp: True
+    type: "box"
+    x: 1.703
+    y: 0.803
+    z: 0.76
+    translation: [0, 0, -0.38] # tx,ty,tz
+    rotation: [0, 0, 0, 1] #qx,qy,qz,qw
+
+collision_pairs:
+    - ["panda_link5_sc_capsule_0", "obstacle1"]
+    - ["panda_link7_sc_0", "obstacle1"]
+    - ["panda_leftfinger_capsule_0", "obstacle1"]
+    - ["panda_link3_sc_capsule_0", "obstacle2"]
+    - ["panda_link5_sc_capsule_0", "obstacle2"]
+    - ["panda_leftfinger_capsule_0", "obstacle2"]
+    - ["panda_link3_sc_capsule_0", "obstacle3"]
+    - ["panda_link5_sc_capsule_0", "obstacle3"]
+    - ["panda_leftfinger_capsule_0", "obstacle3"]

reaching_goal/calibrated-params.yaml

@@ -0,0 +1,8 @@
+# Pose of panda2_ref_camera_link in panda2_hand
+camera:
+  x: 0.13682768843416482
+  y: -0.009443400298528096
+  z: -0.08387833599341328
+  roll: 0.022100209841200636
+  pitch: -1.3156624527412932
+  yaw: 3.061881150974905

reaching_goal/collision_config_safe.yaml

@@ -0,0 +1,10 @@
+collision_config:
+  lower_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+  upper_torque_thresholds_acceleration: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+  lower_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+  upper_torque_thresholds_nominal: [20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0]  # [Nm]
+  lower_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+  upper_force_thresholds_acceleration: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+  lower_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+  upper_force_thresholds_nominal: [20.0, 20.0, 20.0, 25.0, 25.0, 25.0]  # [N, N, N, Nm, Nm, Nm]
+

reaching_goal/collision_config_unsafe.yaml

@@ -0,0 +1,9 @@
+collision_config:
+  lower_torque_thresholds_acceleration: [200.0, 200.0, 180.0, 180.0, 160.0, 140.0, 120.0]  # [Nm]
+  upper_torque_thresholds_acceleration: [200.0, 200.0, 180.0, 180.0, 160.0, 140.0, 120.0]  # [Nm]
+  lower_torque_thresholds_nominal: [200.0, 200.0, 180.0, 180.0, 160.0, 140.0, 120.0]  # [Nm]
+  upper_torque_thresholds_nominal: [200.0, 200.0, 180.0, 180.0, 160.0, 140.0, 120.0]  # [Nm]
+  lower_force_thresholds_acceleration: [200.0, 200.0, 200.0, 250.0, 250.0, 250.0]  # [N, N, N, Nm, Nm, Nm]
+  upper_force_thresholds_acceleration: [200.0, 200.0, 200.0, 250.0, 250.0, 250.0]  # [N, N, N, Nm, Nm, Nm]
+  lower_force_thresholds_nominal: [200.0, 200.0, 200.0, 250.0, 250.0, 250.0]  # [N, N, N, Nm, Nm, Nm]
+  upper_force_thresholds_nominal: [200.0, 200.0, 200.0, 250.0, 250.0, 250.0]  # [N, N, N, Nm, Nm, Nm]