Merge pull request #113 from resibots/devel

Devel

.gitignore

@@ -1,3 +1,5 @@
 build
 CMakeCache.txt
-CMakeFiles
+CMakeFiles
+.vscode/
+etc/talos/randomize.yaml

CMakeLists.txt

@@ -27,7 +27,8 @@ set(INRIA_WBC_SOURCES
     src/behaviors/behavior.cpp
     src/behaviors/generic/cartesian.cpp
     src/behaviors/generic/cartesian_traj.cpp
-    src/behaviors/humanoid/squat.cpp
+    src/behaviors/humanoid/move_com.cpp
+    src/behaviors/humanoid/move_feet.cpp
     src/behaviors/humanoid/walk_on_spot.cpp
     src/behaviors/humanoid/walk.cpp
     src/behaviors/humanoid/clapping.cpp

docs/installation.md

@@ -14,6 +14,26 @@ mkdir install
 sudo apt-get install libyaml-cpp-dev
 ```
 
+## boost-program-options-dev
+
+```
+sudo apt-get install libboost-program-options-dev
+```
+
+## assimp 5.0
+
+```
+git clone --depth 1 https://github.com/assimp/assimp.git
+cd assimp
+git checkout v5.0.0
+mkdir build
+cd build
+cmake ..
+make -j$(nproc)
+sudo make install
+make clean
+```
+
 ## pinocchio
 
 More information on pinnochio [here](https://github.com/stack-of-tasks/pinocchio)

etc/franka/pos_tracker.yaml

@@ -1,6 +1,7 @@
 CONTROLLER:
   name: pos-tracker
   base_path: /home/pal/inria_wbc/etc/franka
+  solver: eiquadprog
   tasks: tasks.yaml
   urdf: franka.urdf
   configurations: configurations.srdf 

etc/icub/pos_tracker.yaml → etc/icub/humanoid_pos_tracker.yaml

@@ -1,5 +1,6 @@
 CONTROLLER:
   name: humanoid-pos-tracker
+  solver: eiquadprog
   base_path: /home/pal/inria_wbc/etc/icub
   tasks: tasks.yaml
   urdf: icub.urdf

etc/icub/squat.yaml

@@ -1,4 +1,7 @@
 BEHAVIOR:
-  name: humanoid::squat
+  name: humanoid::move_com
   trajectory_duration: 1.0
-  motion_size: 0.1
+  targets: [[0, 0, -0.1]]
+  mask: 001
+  absolute: false
+  loop: true

etc/icub/tasks.yaml

@@ -92,8 +92,10 @@ self_collision-left:
         l_knee: 0.08
         r_knee: 0.08
     weight: 500
-    kp: 700.0
-    p: 3
+    kp: 50.0
+    kd: 250.0
+    margin: 0.02
+    m: 0.2
 self_collision-right:
     type: self-collision
     tracked: r_wrist_yaw
@@ -108,5 +110,7 @@ self_collision-right:
         l_knee: 0.08
         r_knee: 0.08
     weight: 500
-    kp: 700.0
-    p: 3
+    kp: 50.0
+    kd: 250.0
+    margin: 0.02
+    m: 0.2

etc/icub/traj_teleop1.yaml

@@ -1,5 +1,5 @@
 BEHAVIOR:
   name : generic::cartesian_traj
-  trajectories: traj_dance/traj_dance.yaml
+  trajectories: traj_teleop1/trajectory.yaml
   loop: true
   scale: 0.75

etc/talos/frames.yaml

@@ -10,4 +10,10 @@ v_base_link_left:
   pos: [0.0, -0.1, 0]
 v_base_link_right:
   ref: "base_link"
-  pos: [0, 0.1, 0]
+  pos: [0, 0.1, 0]
+v_right_gripper:
+  ref: "gripper_right_base_link"
+  pos: [0., 0., -0.09]
+v_left_gripper:
+  ref: "gripper_left_base_link"
+  pos: [0., 0., -0.09]

etc/talos/move_com.yaml

@@ -0,0 +1,7 @@
+BEHAVIOR:
+  name: humanoid::move_com
+  trajectory_duration: 2
+  targets: [[-0.04, 0.0, 0.0],[0.04, 0.0, 0.0],[0.0, 0.0, 0.0]]
+  mask: 100
+  absolute: false
+  loop: true

etc/talos/move_feet.yaml

@@ -0,0 +1,9 @@
+BEHAVIOR:
+  name : humanoid::move-feet
+  task_names: [lf, rf]
+  contact_names: [contact_lfoot, contact_rfoot]
+  relative_targets_pos: [[0.05, 0.05, 0.05], [0.05, 0.05, 0.05]] # empty sublist for no target es. [[1,2,0], []]
+  relative_targets_rpy: [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # empty sublist for no target es. [[], [3.14,0,0]]
+  trajectory_duration: 4
+  loop: false
+

etc/talos/squat.yaml

@@ -1,4 +1,7 @@
 BEHAVIOR:
-  name: humanoid::squat
-  trajectory_duration: 6
-  motion_size: 0.2
+  name: humanoid::move_com
+  trajectory_duration: 2
+  targets: [[0, 0, -0.2]]
+  mask: 001
+  absolute: false
+  loop: true

etc/talos/tasks.yaml

@@ -110,9 +110,11 @@ self_collision-left:
         leg_left_1_joint: 0.15
         leg_right_4_joint: 0.1
         leg_left_4_joint: 0.1
-    weight: 500
-    kp: 700.0
-    p: 3
+    weight: 2000
+    kp: 50.0
+    kd: 250.0
+    margin: 0.02
+    m: 0.2
 self_collision-right:
     type: self-collision
     tracked: gripper_right_joint
@@ -130,28 +132,60 @@ self_collision-right:
         leg_left_1_joint: 0.15
         leg_right_4_joint: 0.1
         leg_left_4_joint: 0.1
-    weight: 500
-    kp: 700.0
-    p: 3
-self_collision-arm-right:
+    weight: 2000
+    kp: 50.0
+    kd: 250.0
+    margin: 0.02
+    m: 0.2
+self_collision-elbow-right:
     type: self-collision
     tracked: arm_right_4_joint
     radius: 0.15
     avoided:
         base_link: 0.15
         v_base_link_left: 0.115
         v_base_link_right: 0.115
-    weight: 10
-    kp: 10.0
-    p: 3
-self_collision-arm-left:
+    weight: 1000
+    kp: 50.0
+    kd: 250.0
+    margin: 0.02
+    m: 0.2
+self_collision-elbow-left:
     type: self-collision
     tracked: arm_left_4_joint
     radius: 0.15
     avoided:
         base_link: 0.15
         v_base_link_left: 0.115
         v_base_link_right: 0.115
-    weight: 10
-    kp: 10.0
-    p: 3
+    weight: 1000
+    kp: 150.0
+    kd: 250.0
+    margin: 0.02
+    m: 0.2
+self_collision-wrist-right:
+    type: self-collision
+    tracked: arm_right_5_joint
+    radius: 0.10
+    avoided:
+        base_link: 0.15
+        v_base_link_left: 0.115
+        v_base_link_right: 0.115
+    weight: 1000
+    kp: 50.0
+    kd: 250.0
+    margin: 0.02
+    m: 0.2
+self_collision-wrist-left:
+    type: self-collision
+    tracked: arm_left_5_joint
+    radius: 0.10
+    avoided:
+        base_link: 0.15
+        v_base_link_left: 0.115
+        v_base_link_right: 0.115
+    weight: 1000
+    kp: 150.0
+    kd: 250.0
+    margin: 0.02
+    m: 0.2

etc/talos/traj_teleop1.yaml

@@ -1,5 +1,5 @@
 BEHAVIOR:
   name : generic::cartesian_traj
-  trajectories: traj_dance/traj_dance.yaml
+  trajectories: traj_teleop1/trajectory.yaml
   loop: true
   scale: 1

etc/tiago/cartesian_line.yaml

@@ -0,0 +1,7 @@
+BEHAVIOR:
+  name : generic::cartesian
+  task_names: [ee]
+  trajectory_duration: 5.0
+  relative_targets_pos: [[0., 0.5, 0.5]]
+  relative_targets_rpy: [[0.0, 0.0, 0.0]]
+  loop: true

etc/tiago/collisions.yaml

@@ -0,0 +1,12 @@
+members:
+    arm:
+        arm_3_link: [[0., 0. ,0., 0.13], [0., 0. ,-0.1, 0.16]]
+        arm_4_link: [[0., 0. ,0.025, 0.2], [-0.1, 0.0 ,0.025, 0.2]]
+        arm_5_link: [[0., 0. , 0.03, 0.18],[0.0, 0.0 ,0.15, 0.18]]
+        arm_7_link: [[0., 0., 0.1, 0.20], [0., 0., 0.2, 0.10]]
+    torso:
+        base_link: [[0., 0., 0.11, 0.67]]
+        torso_lift_link: [[0.0, 0., 0.03, 0.52],[0., 0., -0.2, 0.36],[0., 0., -0.3, 0.36], [0., 0., -0.45, 0.36], [0., 0., -0.55, 0.36], [0., 0., -0.7, 0.36]]
+        head_2_link: [[0., 0.05, 0.0, 0.35]]
+
+

etc/tiago/configurations-elbow-down.srdf

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<robot name="tiago">
+    <group_state name="start" group="all">
+        <joint name="wheel_right_joint" value="0.0" />
+        <joint name="wheel_left_joint" value="0.0" />
+        <joint name="torso_lift_joint" value="0.0" />
+        <joint name="head_1_joint" value="0.0" />
+        <joint name="head_2_joint" value="0.0" />
+        <joint name="arm_1_joint" value="1.57079632679"/>
+        <joint name="arm_2_joint" value="-0.78539816339" />
+        <joint name="arm_3_joint" value="-3.14" />
+        <joint name="arm_4_joint" value="1.57079632679" />
+        <joint name="arm_5_joint" value="1.57079632679" />
+        <joint name="arm_6_joint" value="0.78539816339" />
+        <joint name="arm_7_joint" value="0.0" />
+        <joint name="gripper_finger_joint" value="0.0" />
+    </group_state>
+</robot>

etc/tiago/configurations-elbow-up.srdf

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<robot name="tiago">
+    <group_state name="start" group="all">
+        <joint name="wheel_right_joint" value="0.0" />
+        <joint name="wheel_left_joint" value="0.0" />
+        <joint name="torso_lift_joint" value="0.0" />
+        <joint name="head_1_joint" value="0.0" />
+        <joint name="head_2_joint" value="0.0" />
+        <joint name="arm_1_joint" value="1.57079632679"/>
+        <joint name="arm_2_joint" value="0.78539816339" />
+        <joint name="arm_3_joint" value="0.0" />
+        <joint name="arm_4_joint" value="1.57079632679" />
+        <joint name="arm_5_joint" value="1.57079632679" />
+        <joint name="arm_6_joint" value="0.78539816339" />
+        <joint name="arm_7_joint" value="0.0" />
+        <joint name="gripper_finger_joint" value="0.0" />
+    </group_state>
+</robot>

etc/tiago/frames.yaml

@@ -0,0 +1,10 @@
+# virtual frames
+v_torso_1:
+  ref: "torso_lift_link"
+  pos: [0.0, 0.0, -0.2]
+v_torso_2:
+  ref: "torso_lift_link"
+  pos: [0.0, 0.0, -0.4]
+v_head:
+  ref: "head_2_link"
+  pos: [0.04, 0.05, 0.0]

etc/tiago/pos_tracker.yaml

@@ -0,0 +1,17 @@
+CONTROLLER:
+  name: pos-tracker
+  base_path: /home/pal/inria_wbc/etc/tiago
+  tasks: tasks.yaml
+  frames: frames.yaml
+  urdf: tiago_steel.urdf
+  configurations: configurations-elbow-down.srdf 
+  closed_loop: false
+  solver: eiquadprog
+  ref_config: start
+  floating_base: false
+  floating_base_joint_name: ""
+  dt: 0.001
+  verbose: false
+  mimic_dof_names : []
+  check_model_collisions: true
+  collision_path: collisions.yaml