diff --git a/bioloid_control/config/dexter.yaml b/bioloid_control/config/dexter.yaml
index f87ff5eceed016393ceecdc9c15328485e32f784..d61ddc195ac00a5b966f3f568c08aaaeee4c5c69 100644
--- a/bioloid_control/config/dexter.yaml
+++ b/bioloid_control/config/dexter.yaml
@@ -7,19 +7,20 @@ bioloid:
bioloid_cm510_cont:
type: effort_controllers/BioloidControllerCM510
exp_adc8_frame: IR1_link
- exp_adc7_frame: IRL1_link
+ exp_adc7_frame: IR2_link
exp_adc6_frame: LAT_IR_left_link
exp_adc5_frame: LAT_IR_right_link
+ exp_adc4_frame: IRLONG_link
- adc3_frame: gyro_x
- adc4_frame: gyro_y
+ adc3_frame: gyro_y
+ adc4_frame: gyro_x
exp_board_id: 192
- exp_gpio0_frame: left_front_fwd_ir_link
- exp_gpio1_frame: left_front_dwn_ir_link
- exp_gpio2_frame: left_lateral_dwn_ir_link
- exp_gpio3_frame: right_front_fwd_ir_link
- exp_gpio4_frame: right_front_dwn_ir_link
- exp_gpio5_frame: right_lateral_dwn_ir_link
+ exp_gpio1_frame: left_front_fwd_ir_link
+ exp_gpio2_frame: left_front_dwn_ir_link
+ exp_gpio3_frame: left_lateral_dwn_ir_link
+ exp_gpio18_frame: right_front_fwd_ir_link
+ exp_gpio17_frame: right_front_dwn_ir_link
+ exp_gpio16_frame: right_lateral_dwn_ir_link
joints:
- j_shoulder_r
- j_shoulder_l
diff --git a/bioloid_control/config/dexter_pan_tilt.yaml b/bioloid_control/config/dexter_pan_tilt.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..b76447f3c882d6f053b2412def59892135517809
--- /dev/null
+++ b/bioloid_control/config/dexter_pan_tilt.yaml
@@ -0,0 +1,148 @@
+bioloid:
+ # Publish all joint states -----------------------------------
+ joint_state_controller:
+ type: joint_state_controller/JointStateController
+ publish_rate: 50
+
+ bioloid_cm510_cont:
+ type: effort_controllers/BioloidControllerCM510
+ exp_adc8_frame: IR1_link
+ exp_adc7_frame: IR2_link
+ exp_adc6_frame: LAT_IR_left_link
+ exp_adc5_frame: LAT_IR_right_link
+ exp_adc4_frame: IRLONG_link
+
+ adc3_frame: gyro_y
+ adc4_frame: gyro_x
+ exp_board_id: 192
+ exp_gpio1_frame: left_front_fwd_ir_link
+ exp_gpio2_frame: left_front_dwn_ir_link
+ exp_gpio3_frame: left_lateral_dwn_ir_link
+ exp_gpio18_frame: right_front_fwd_ir_link
+ exp_gpio17_frame: right_front_dwn_ir_link
+ exp_gpio16_frame: right_lateral_dwn_ir_link
+ pan_servo_id: 19
+ tilt_servo_id: 20
+ serial_console_device: /dev/pts/18
+ joints:
+ - j_shoulder_r
+ - j_shoulder_l
+ - j_high_arm_r
+ - j_high_arm_l
+ - j_low_arm_r
+ - j_low_arm_l
+ - j_pelvis_yaw_r
+ - j_pelvis_yaw_l
+ - j_pelvis_roll_r
+ - j_pelvis_roll_l
+ - j_pelvis_pitch_r
+ - j_pelvis_pitch_l
+ - j_knee_r
+ - j_knee_l
+ - j_ankle_pitch_r
+ - j_ankle_pitch_l
+ - j_ankle_roll_r
+ - j_ankle_roll_l
+ - j_pan
+ - j_tilt
+ gains:
+ j_shoulder_l:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_high_arm_l:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_low_arm_l:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_shoulder_r:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_high_arm_r:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_low_arm_r:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_pelvis_yaw_l:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_pelvis_roll_l:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_pelvis_pitch_l:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_knee_l:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_ankle_pitch_l:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_ankle_roll_l:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_pelvis_yaw_r:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_pelvis_roll_r:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_pelvis_pitch_r:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_knee_r:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_ankle_pitch_r:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_ankle_roll_r:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_pan:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
+ j_tilt:
+ punch: 32
+ margin: 1
+ slope: 32
+ max_torque: 1.5
diff --git a/bioloid_description/urdf/dexter.xacro b/bioloid_description/urdf/dexter.xacro
index 525f0c8d09cd05153e150219b6cb7ddcce1aa810..bf84d957f2da53b89e62f0cc4d93e862bce5e79e 100755
--- a/bioloid_description/urdf/dexter.xacro
+++ b/bioloid_description/urdf/dexter.xacro
@@ -4,13 +4,15 @@
<xacro:include filename="$(find bioloid_description)/urdf/bioloid_ceabot.gazebo" />
<xacro:include filename="$(find bioloid_description)/urdf/sensors/sharp_ir.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/sensors/feet_ir.xacro" />
+ <xacro:include filename="$(find bioloid_description)/urdf/pan_tilt.xacro" />
+ <xacro:include filename="$(find bioloid_description)/urdf/sensors/ids_xs.xacro" />
<xacro:sharp_ir name="IR1" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
- <origin xyz="-0.04 0.00 0.035" rpy="0 -1.8326 0" />
+ <origin xyz="-0.04 0.00 0.035" rpy="0 -1.745 0" />
</xacro:sharp_ir>
<xacro:sharp_ir name="IR2" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
- <origin xyz="0.04 0.00 0.035" rpy="0 -1.3090 0" />
+ <origin xyz="0.04 0.00 0.035" rpy="0 -1.396 0" />
</xacro:sharp_ir>
<xacro:sharp_ir name="IRLONG" parent="base_link" update_rate="20" fov="0.05" min_range="0.20" max_range="1.5">
@@ -19,16 +21,12 @@
<!-- this is for the lateral sensors-->
- <xacro:sharp_ir name="LAT_IR_left" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
+ <!-- <xacro:sharp_ir name="LAT_IR_left" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
<origin xyz="0.04 0.03 0.035" rpy="-1.5707 0 0" />
</xacro:sharp_ir>
<xacro:sharp_ir name="LAT_IR_right" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
<origin xyz="-0.04 0.03 0.035" rpy="-1.5707 3.1416 0" />
- </xacro:sharp_ir>
-
-
-
-
+ </xacro:sharp_ir> -->
<xacro:feet_ir left_parent="left_leg_ankle_roll" right_parent="right_leg_ankle_roll" update_rate="20" range="0.01"/>
@@ -89,5 +87,13 @@
<child link="controller"/>
<origin xyz="0 -0.01 -0.022" rpy="-1.5707 -1.5707 0" />
</joint>
+ <xacro:if value="$(arg use_pan_tilt)">
+ <xacro:pan_tilt parent="base_link">
+ <origin xyz="0.0 0.022 0.005" rpy="1.5707 0 3.1415" />
+ </xacro:pan_tilt>
+ <xacro:ids_xs name="bioloid_cam" parent="head">
+ <origin xyz="0.02 0.01 0.0" rpy="-1.5707 0 0" />
+ </xacro:ids_xs>
+ </xacro:if>
</robot>