diff --git a/tor_bringup/config/tor_hardware.yaml b/tor_bringup/config/tor_hardware.yaml index c0f6e328c20077d92a7bd1c54633260d589e625f..7916d061d506c7b40ee4b152845069ec25d88cc2 100644 --- a/tor_bringup/config/tor_hardware.yaml +++ b/tor_bringup/config/tor_hardware.yaml @@ -13,13 +13,19 @@ e_stop: e_stop_port: emergency_stop_state force_torque: - left_ft: + left_ankle_ft: frame: leg_left_6_link + transformation: + force: [x,-y,-z] + torque: [x,-y,-z] raw_data: force_port: force_ankle_left torque_port: torque_ankle_left - right_ft: + right_ankle_ft: frame: leg_right_6_link + transformation: + force: [x,-y,-z] + torque: [x,-y,-z] raw_data: force_port: force_ankle_right torque_port: torque_ankle_right diff --git a/tor_controller_configuration/config/init_offset_controller.yaml b/tor_controller_configuration/config/init_offset_controller.yaml new file mode 100644 index 0000000000000000000000000000000000000000..3ac7355bcb2c18dd839c771d8a8231dec225883d --- /dev/null +++ b/tor_controller_configuration/config/init_offset_controller.yaml @@ -0,0 +1,19 @@ +init_offset_controller: + type: reemc_controllers/ReemcInitOffsetController + joints: + - leg_left_1_joint + - leg_left_2_joint + - leg_left_3_joint + - leg_left_4_joint + - leg_left_5_joint + - leg_left_6_joint + - leg_right_1_joint + - leg_right_2_joint + - leg_right_3_joint + - leg_right_4_joint + - leg_right_5_joint + - leg_right_6_joint + + left_ft_sensor: left_ankle_ft + right_ft_sensor: right_ankle_ft + base_imu_sensor: base_imu diff --git a/tor_controller_configuration/config/joint_trajectory_controllers.yaml b/tor_controller_configuration/config/joint_trajectory_controllers.yaml index fbdd8e072c8a2cbe95c72beaea0ae32392654bda..82efac763aeb6053e8e4d7e49f57e4f1661703db 100644 --- a/tor_controller_configuration/config/joint_trajectory_controllers.yaml +++ b/tor_controller_configuration/config/joint_trajectory_controllers.yaml @@ -9,7 +9,7 @@ left_leg_controller: - leg_left_6_joint constraints: goal_time: &goal_time_constraint 0.6 - stopped_velocity_tolerance: *stopped_velocity_constraint + stopped_velocity_tolerance: &stopped_velocity_constraint 0.05 arm_left_1_joint: goal: &goal_pos_constraint 0.02 arm_left_2_joint: diff --git a/tor_controller_configuration/launch/tor_homing_controller.launch b/tor_controller_configuration/launch/tor_homing_controller.launch new file mode 100644 index 0000000000000000000000000000000000000000..693554fbb88fd712360e6e37a7121dc0df408907 --- /dev/null +++ b/tor_controller_configuration/launch/tor_homing_controller.launch @@ -0,0 +1,10 @@ +<launch> + + <rosparam command="load" file="$(find tor_controller_configuration)/config/homing_controller.yaml" /> + + <!-- Controllers that come up started --> + <node name="controllers_spawner" + pkg="controller_manager" type="spawner" output="screen" + args="homing_controller" /> + +</launch>