Update to changes in roscontrol_sot and dynamic_graph_bridge.

roscontrol_sot_tiago/config/pids_velocity.yaml

+gains:
+  wheel_left_joint:  {p: 1, d: 0.0, i: 0, i_clamp: 1.5 }
+  wheel_right_joint: {p: 1, d: 0.0, i: 0, i_clamp: 1.5 }

roscontrol_sot_tiago/config/sot_tiago_params_gazebo.yaml

 sot_controller:
   libname: libsot-tiago-steel-controller.so
   simulation_mode: gazebo
-  joint_names: [ torso_lift_joint,
+  joint_names: [
+    wheel_left_joint, wheel_right_joint,
+    torso_lift_joint,
     arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint, gripper_finger_joint,
     head_1_joint, head_2_joint  ]
   map_rc_to_sot_device: { motor-angles: motor-angles ,
   joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
-  torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
-  position_control_init_pos:
-    - name: torso_lift_joint
-      des_pos: 0.0
-    - name: arm_1_joint
-      des_pos: 0.25847
-    - name: arm_2_joint
-      des_pos: 0.173046
-    - name: arm_3_joint
-      des_pos: 0.0002
-    - name: arm_4_joint
-      des_pos: -0.525366
-    - name: arm_5_joint
-      des_pos: 0.0
-    - name: arm_6_joint
-      des_pos: 0.0
-    - name: arm_7_joint
-      des_pos: 0.1
-    - name: gripper_finger_joint
-      des_pos: 0.034
-    - name: head_1_joint
-      des_pos: 0.0
-    - name: head_2_joint
-      des_pos: 0.0   
-  control_mode: POSITION
+  torques: torques, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0,
+  control: control
+  }
+  control_mode:
+    wheel_left_joint:
+      ros_control_mode: VELOCITY
+    wheel_right_joint:
+      ros_control_mode: VELOCITY
+    torso_lift_joint:
+      ros_control_mode: POSITION
+    arm_1_joint:
+      ros_control_mode: POSITION
+    arm_2_joint:
+      ros_control_mode: POSITION
+    arm_3_joint:
+      ros_control_mode: POSITION
+    arm_4_joint:
+      ros_control_mode: POSITION
+    arm_5_joint:
+      ros_control_mode: POSITION
+    arm_6_joint:
+      ros_control_mode: POSITION
+    arm_7_joint:
+      ros_control_mode: POSITION
+    gripper_finger_joint:
+      ros_control_mode: POSITION
+    head_1_joint:
+      ros_control_mode: POSITION
+    head_2_joint:
+      ros_control_mode: POSITION
   dt: 0.001
   jitter: 0.0004

roscontrol_sot_tiago/launch/sot_tiago_controller_gazebo.launch

@@ -2,6 +2,7 @@
 
   <!-- Sot Controller configuration -->
   <rosparam command="load" file="$(find roscontrol_sot_tiago)/config/sot_tiago_params_gazebo.yaml"/>
+  <rosparam command="load" ns="/sot_controller/velocity_control_pd_motor_init" file="$(find roscontrol_sot_tiago)/config/pids_velocity.yaml"/>
   <rosparam command="load" file="$(find roscontrol_sot_tiago)/config/sot_tiago_controller.yaml" />
   
   <!-- Spawn walking controller -->

sot_tiago_bringup/sot/tiago.yaml

-sot:
-  state_vector_map: [  torso_lift_joint,
-    arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint, gripper_finger_joint,
-    head_1_joint,head_2_joint]
 sot_controller:
-  joint_names: [  torso_lift_joint,
+  joint_names: [
+    wheel_left_joint, wheel_right_joint,
+    torso_lift_joint,
     arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint, gripper_finger_joint,
     head_1_joint,head_2_joint
   ]