diff --git a/bioloid_control/config/dexter.yaml b/bioloid_control/config/dexter.yaml index d008e0334f12a9cbb87963df0a80d3681ca85d46..f87ff5eceed016393ceecdc9c15328485e32f784 100644 --- a/bioloid_control/config/dexter.yaml +++ b/bioloid_control/config/dexter.yaml @@ -8,10 +8,11 @@ bioloid: type: effort_controllers/BioloidControllerCM510 exp_adc8_frame: IR1_link exp_adc7_frame: IRL1_link + exp_adc6_frame: LAT_IR_left_link + exp_adc5_frame: LAT_IR_right_link + adc3_frame: gyro_x adc4_frame: gyro_y - exp_adc1_frame: IR2_link - exp_adc8_frame: IR3_link exp_board_id: 192 exp_gpio0_frame: left_front_fwd_ir_link exp_gpio1_frame: left_front_dwn_ir_link diff --git a/bioloid_description/urdf/dexter.xacro b/bioloid_description/urdf/dexter.xacro index 9b3214885574dc9ee226b8a292604224f9a9e212..da58db41b43b8c4f7a40959d19ff58a666870079 100755 --- a/bioloid_description/urdf/dexter.xacro +++ b/bioloid_description/urdf/dexter.xacro @@ -10,16 +10,28 @@ <origin xyz="0 0.0 0.035" rpy="-1.5707 -1.5707 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"> + <xacro:sharp_ir_long name="IRL1" parent="base_link" update_rate="20" fov="0.05" min_range="0.20" max_range="1.5"> <origin xyz="0 -0.05 0.035" rpy="-1.5707 -1.5707 0" /> + </xacro:sharp_ir_long> + +<!-- <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 -0.05 0.035" rpy="-1.5707 -1.5707 0" /> + </xacro:sharp_ir> --> + + <!-- this is for the lateral sensors 3d component puts sensor vertical--> + + <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.05 0.04 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.03 0.04 0.035" rpy="-1.5707 3.1416 0" /> </xacro:sharp_ir> - <xacro:sharp_ir_long name="IRL1" parent="base_link" update_rate="20" fov="0.05" min_range="0.20" max_range="1.5"> - <origin xyz="0 0.05 0.035" rpy="-1.5707 -1.5707 0" /> - </xacro:sharp_ir_long> -<!-- <xacro:feet_ir left_parent="left_leg_ankle_roll" right_parent="right_leg_ankle_roll" update_rate="20" range="0.01"/>--> + + + <xacro:feet_ir left_parent="left_leg_ankle_roll" right_parent="right_leg_ankle_roll" update_rate="20" range="0.01"/> <link name="battery_pack"> <collision>