From 56b046b4c6546b8143c3036b3fb01828592174ad Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Mon, 25 Sep 2017 15:26:06 +0200
Subject: [PATCH] Fix wrong type of interface provided if EFFORT mode is switch
on. Add proper launch files for effor in simulation.
---
.../src/roscontrol-sot-controller.cpp | 10 ++++++++--
.../config/sot_talos_params_gazebo_effort.yaml | 13 +++++++++++++
.../sot_talos_controller_gazebo_effort.launch | 13 +++++++++++++
3 files changed, 34 insertions(+), 2 deletions(-)
create mode 100644 talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml
create mode 100644 talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo_effort.launch
diff --git a/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp b/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
index 56b1de4..98d90ea 100644
--- a/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
+++ b/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
@@ -31,7 +31,7 @@
DebugFile << x << std::endl; \
DebugFile.close();}
-#define RESERDEBUG4()
+#define RESETDEBUG4()
#define ODEBUG4FULL(x)
#define ODEBUG4(x)
@@ -50,6 +50,7 @@ namespace talos_sot_controller
simulation_mode_(false),
control_mode_(POSITION)
{
+ RESETDEBUG4();
}
void RCSotController::
@@ -662,7 +663,12 @@ namespace talos_sot_controller
getHardwareInterfaceType() const
{
//return type_name_;
- return talos_hardware_interface::internal::demangledTypeName<talos_hardware_interface::PositionJointInterface>();
+ if (control_mode_==POSITION)
+ return talos_hardware_interface::internal::
+ demangledTypeName<talos_hardware_interface::PositionJointInterface>();
+ else if (control_mode_==EFFORT)
+ return talos_hardware_interface::internal::
+ demangledTypeName<talos_hardware_interface::EffortJointInterface>();
}
diff --git a/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml b/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml
new file mode 100644
index 0000000..5a9cb80
--- /dev/null
+++ b/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml
@@ -0,0 +1,13 @@
+sot_controller:
+ libname: libsot-pyrene-controller.so
+ simulation_mode: gazebo
+ joint_names: [ 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,
+ torso_1_joint,torso_2_joint,
+ arm_left_1_joint, arm_left_2_joint, arm_left_3_joint, arm_left_4_joint, arm_left_5_joint, arm_left_6_joint, arm_left_7_joint, gripper_left_joint,
+ arm_right_1_joint, arm_right_2_joint, arm_right_3_joint, arm_right_4_joint, arm_right_5_joint, arm_right_6_joint, arm_right_7_joint, gripper_right_joint,
+ head_1_joint, head_2_joint ]
+ map_rc_to_sot_device: { motor-angles: motor-angles ,
+ joint-angles: joint-angles, velocities: velocities,
+ torques: torques, cmd-joints: control, cmd-effort: control }
+ control_mode: EFFORT
diff --git a/talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo_effort.launch b/talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo_effort.launch
new file mode 100644
index 0000000..9f5f1f5
--- /dev/null
+++ b/talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo_effort.launch
@@ -0,0 +1,13 @@
+<launch>
+
+ <!-- Sot Controller configuration -->
+ <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params_gazebo_effort.yaml"/>
+ <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
+
+ <!-- Spawn walking controller -->
+ <node name="sot_controller_spawner"
+ pkg="talos_controller_manager" type="spawner" output="screen"
+ args="sot_controller" />
+
+</launch>
+
--
GitLab