Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • gsaurel/talos-metapkg-ros-control-sot
  • stack-of-tasks/talos-metapkg-ros-control-sot
2 results
Show changes
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 1548 deletions
talos_roscontrol_sot/doc/manifest.yaml deleted 100644 → 0
View file @ 5f8a699d
actions: []
authors: Olivier Stasse <ostasse@laas.fr>
brief: ''
bugtracker: ''
depends:
- controller_interface
- roscpp
- std_msgs
- sensor_msgs
- realtime_tools
- catkin
- cmake_modules
- rospy
- control_msgs
description: Wrapping the Stack-of-tasks framework in ros-control
license: BSD
maintainers: Olivier Stasse <ostasse@laas.fr>
msgs: []
package_type: package
repo_url: ''
srvs: []
url: ''
talos_roscontrol_sot/launch/sot_controller_gazebo.launch deleted 100644 → 0
View file @ 5f8a699d
<launch>
<!-- Sot Controller configuration -->
<!-- <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_your_robot_params_gazebo.yaml"/> -->
<!-- <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_your_robot_controller.yaml" /> -->
<!-- Spawn walking controller -->
<node name="sot_controller_spawner"
pkg="talos_controller_manager" type="spawner" output="screen"
args="sot_controller" />
</launch>
talos_roscontrol_sot/package.xml deleted 100644 → 0
View file @ 5f8a699d
<?xml version="1.0"?>
<package>
<name>talos_roscontrol_sot</name>
<version>0.0.6</version>
<description>Wrapping the Stack-of-tasks framework in ros-control for talos</description>
<!-- Maintainer email -->
<maintainer email="ostasse@laas.fr">Olivier Stasse</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<license>BSD</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/roscontrol_sot</url> -->
<!-- Author tag -->
<author email="ostasse@laas.fr">Olivier Stasse</author>
<!-- The *_depend tags are used to specify dependencies -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>control_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>realtime_tools</build_depend>
<build_depend>controller_interface</build_depend>
<build_depend>pal_hardware_interfaces</build_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>dynamic_graph_bridge</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>control_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>realtime_tools</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>controller_interface</run_depend>
<run_depend>pal_hardware_interfaces</run_depend>
<run_depend>cmake_modules</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>dynamic_graph_bridge</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<controller_interface plugin="${prefix}/config/roscontrol_sot_controller_plugins.xml"/>
</export>
</package>
talos_roscontrol_sot/src/log.cpp deleted 100644 → 0
View file @ 5f8a699d
/*
Olivier Stasse CNRS,
31/07/2012
Object to log the low-level informations of a robot.
*/
#include "log.hh"
#include <sys/time.h>
#include <sstream>
#include <fstream>
#include <iostream>
using namespace std;
using namespace rc_sot_system;
DataToLog::DataToLog()
{
}
void DataToLog::init(unsigned int nbDofs,long int length)
{
motor_angle.resize(nbDofs*length);
joint_angle.resize(nbDofs*length);
velocities.resize(nbDofs*length);
torques.resize(nbDofs*length);
motor_currents.resize(nbDofs*length);
orientation.resize(4*length);
accelerometer.resize(3*length);
gyrometer.resize(3*length);
force_sensors.resize(4*6*length);
temperatures.resize(nbDofs*length);
timestamp.resize(length);
for(unsigned int i=0;i<nbDofs*length;i++)
{ motor_angle[i] = joint_angle[i] =
velocities[i] = 0.0; }
}
Log::Log():
lref_(0),
lrefts_(0)
{
}
void Log::init(unsigned int nbDofs, unsigned int length)
{
lref_ =0;
lrefts_=0;
nbDofs_=nbDofs;
length_=length;
StoredData_.init(nbDofs,length);
struct timeval current;
gettimeofday(&current,0);
timeorigin_ = (double)current.tv_sec + 0.000001 * ((double)current.tv_usec);
}
void Log::record(DataToLog &aDataToLog)
{
if ( (aDataToLog.motor_angle.size()!=nbDofs_) ||
(aDataToLog.velocities.size()!=nbDofs_))
return;
for(unsigned int JointID=0;JointID<nbDofs_;JointID++)
{
if (aDataToLog.motor_angle.size()>JointID)
StoredData_.motor_angle[JointID+lref_]= aDataToLog.motor_angle[JointID];
if (aDataToLog.joint_angle.size()>JointID)
StoredData_.joint_angle[JointID+lref_]= aDataToLog.joint_angle[JointID];
if (aDataToLog.velocities.size()>JointID)
StoredData_.velocities[JointID+lref_]= aDataToLog.velocities[JointID];
if (aDataToLog.torques.size()>JointID)
StoredData_.torques[JointID+lref_]= aDataToLog.torques[JointID];
if (aDataToLog.motor_currents.size()>JointID)
StoredData_.motor_currents[JointID+lref_]= aDataToLog.motor_currents[JointID];
if (aDataToLog.temperatures.size()>JointID)
StoredData_.temperatures[JointID+lref_]= aDataToLog.temperatures[JointID];
}
for(unsigned int axis=0;axis<3;axis++)
{
StoredData_.accelerometer[lrefts_*3+axis] = aDataToLog.accelerometer[axis];
StoredData_.gyrometer[lrefts_*3+axis] = aDataToLog.gyrometer[axis];
}
for(unsigned int fsID=0;fsID<4;fsID++)
{
for(unsigned int axis=0;axis<6;axis++)
{
StoredData_.force_sensors[lrefts_*24+fsID*6+axis] =
aDataToLog.force_sensors[fsID*6+axis];
}
}
struct timeval current;
gettimeofday(&current,0);
StoredData_.timestamp[lrefts_] =
((double)current.tv_sec + 0.000001 * (double)current.tv_usec) - timeorigin_;
lref_ += nbDofs_;
lrefts_ ++;
if (lref_>=nbDofs_*length_)
{
lref_=0;
lrefts_=0;
}
}
void Log::save(std::string &fileName)
{
std::string suffix("-mastate.log");
saveVector(fileName,suffix,StoredData_.motor_angle, nbDofs_);
suffix="-jastate.log";
saveVector(fileName,suffix,StoredData_.joint_angle, nbDofs_);
suffix = "-vstate.log";
saveVector(fileName,suffix,StoredData_.velocities, nbDofs_);
suffix = "-torques.log";
saveVector(fileName,suffix,StoredData_.torques, nbDofs_);
suffix = "-motor-currents.log";
saveVector(fileName,suffix,StoredData_.motor_currents, nbDofs_);
suffix = "-accelero.log";
saveVector(fileName,suffix,StoredData_.accelerometer, 3);
suffix = "-gyro.log";
saveVector(fileName,suffix,StoredData_.gyrometer, 3);
ostringstream oss;
oss << "-forceSensors.log";
suffix = oss.str();
saveVector(fileName,suffix,StoredData_.force_sensors, 6);
suffix = "-temperatures.log";
saveVector(fileName,suffix,StoredData_.temperatures, nbDofs_);
}
void Log::saveVector(std::string &fileName,std::string &suffix,
std::vector<double> &avector,
unsigned int size)
{
ostringstream oss;
oss << fileName;
oss << suffix.c_str();
std::string actualFileName= oss.str();
ofstream aof(actualFileName.c_str());
if (aof.is_open())
{
for(unsigned long int i=0;i<length_;i++)
{
// Save timestamp
aof << StoredData_.timestamp[i] << " " ;
// Compute and save dt
if (i==0)
aof << StoredData_.timestamp[i] - StoredData_.timestamp[length_-1] << " ";
else
aof << StoredData_.timestamp[i] - StoredData_.timestamp[i-1] << " ";
// Save all data
for(unsigned long int datumID=0;datumID<size;datumID++)
aof << avector[i*size+datumID] << " " ;
aof << std::endl;
}
aof.close();
}
}
talos_roscontrol_sot/src/log.hh deleted 100644 → 0
View file @ 5f8a699d
/*
Olivier Stasse CNRS,
19/12/2016
Object to control the low-level part of TALOS.
*/
#ifndef _RC_SOT_SYSTEM_LOG_H_
#define _RC_SOT_SYSTEM_LOG_H_
#include <vector>
#include <string>
namespace rc_sot_system {
struct DataToLog
{
// Measured angle values at the motor side.
std::vector<double> motor_angle;
// Measured angle at the joint side.
std::vector<double> joint_angle;
// Measured or computed velocities.
std::vector<double> velocities;
// Measured torques.
std::vector<double> torques;
// Reconstructed orientation (from internal IMU).
std::vector<double> orientation;
// Measured linear acceleration
std::vector<double> accelerometer;
// Measured angular velocities
std::vector<double> gyrometer;
// Measured force sensors
std::vector<double> force_sensors;
// Measured motor currents
std::vector<double> motor_currents;
// Measured temperatures
std::vector<double> temperatures;
// Timestamp
std::vector<double> timestamp;
DataToLog();
void init(unsigned int nbDofs, long int length);
};
class Log
{
private:
// Actuated informations logged.
unsigned int nbDofs_;
// Number of iterations to be logged.
unsigned int length_;
// Current position in the circular buffer for angles.
long unsigned int lref_;
// Current position int the circular buffer for timestamp
// lref_ = lrefts_ * nbDofs_
long unsigned int lrefts_;
// Circular buffer for all the data.
DataToLog StoredData_;
double timeorigin_;
// Save one vector of information.
void saveVector(std::string &filename,
std::string &suffix,
std::vector<double> &avector,
unsigned int);
public:
Log();
void init(unsigned int nbDofs, unsigned int length);
void record(DataToLog &aDataToLog);
void save(std::string &fileName);
};
}
#endif /* _RC_SOT_SYSTEM_LOG_H_ */
talos_roscontrol_sot/src/roscontrol-sot-controller.hh deleted 100644 → 0
View file @ 5f8a699d
///////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2016, CNRS
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of hiDOF, Inc. nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//////////////////////////////////////////////////////////////////////////////
/*
* Author: Olivier STASSE
*/
#ifndef RC_SOT_CONTROLLER_H
#define RC_SOT_CONTROLLER_H
#include <string>
#include <map>
#ifdef REAL_ROBOT
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/imu_sensor_interface.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <pal_hardware_interfaces/actuator_temperature_interface.h>
#else
#include <talos_controller_interface/controller.h>
#include <talos_hardware_interface/joint_command_interface.h>
#include <talos_hardware_interface/imu_sensor_interface.h>
#include <talos_hardware_interface/force_torque_sensor_interface.h>
#include <talos_pal_hardware_interfaces/actuator_temperature_interface.h>
#endif
#include <dynamic_graph_bridge/sot_loader_basic.hh>
#include <ros/ros.h>
#include <control_toolbox/pid.h>
#include "log.hh"
namespace talos_sot_controller
{
enum SotControlMode { POSITION, EFFORT};
class XmlrpcHelperException : public ros::Exception
{
public:
XmlrpcHelperException(const std::string& what)
: ros::Exception(what) {}
};
struct EffortControlPDMotorControlData
{
control_toolbox::Pid pid_controller;
//double p_gain,d_gain,i_gain;
double prev;
double vel_prev;
double des_pos;
double integ_err;
EffortControlPDMotorControlData();
// void read_from_xmlrpc_value(XmlRpc::XmlRpcValue &aXRV);
void read_from_xmlrpc_value(const std::string &prefix);
};
/**
This class encapsulates the Stack of Tasks inside the ros-control infra-structure.
*/
#ifdef REAL_ROBOT
namespace lhi = hardware_interface;
namespace lci = controller_interface;
#else
namespace lhi = talos_hardware_interface;
namespace lci = talos_controller_interface;
#endif
class RCSotController : public lci::ControllerBase,
SotLoaderBasic
{
protected:
/// Robot nb dofs.
size_t nbDofs_;
/// Data log.
rc_sot_system::DataToLog DataOneIter_;
private:
/// @{ \name Ros-control related fields
/// \brief Vector of joint handles.
std::vector<lhi::JointHandle> joints_;
std::vector<std::string> joints_name_;
/// \brief Vector towards the IMU.
std::vector<lhi::ImuSensorHandle> imu_sensor_;
/// \brief Vector of 6D force sensor.
std::vector<lhi::ForceTorqueSensorHandle> ft_sensors_;
/// \brief Vector of temperature sensors for the actuators.
std::vector<lhi::ActuatorTemperatureSensorHandle>
act_temp_sensors_;
/// \brief Interface to the joints controlled in position.
lhi::PositionJointInterface * pos_iface_;
/// \brief Interface to the joints controlled in force.
lhi::EffortJointInterface * effort_iface_;
/// \brief Interface to the sensors (IMU).
lhi::ImuSensorInterface* imu_iface_;
/// \brief Interface to the sensors (Force).
lhi::ForceTorqueSensorInterface* ft_iface_;
/// \brief Interface to the actuator temperature sensor.
lhi::ActuatorTemperatureSensorInterface * act_temp_iface_;
/// @}
/// \brief Log
rc_sot_system::Log RcSotLog;
/// @}
const std::string type_name_;
/// \brief Adapt the interface to Gazebo simulation
bool simulation_mode_;
/// \brief The robot can controlled in effort or position mode (default).
SotControlMode control_mode_;
/// \brief Implement a PD controller for the robot when the dynamic graph
/// is not on.
std::map<std::string,EffortControlPDMotorControlData> effort_mode_pd_motors_;
/// \brief Map from ros-control quantities to robot device
/// ros-control quantities are for the sensors:
/// * motor-angles
/// * joint-angles
/// * velocities
/// * torques
/// ros-control quantities for control are:
/// * joints
/// * torques
std::map<std::string,std::string> mapFromRCToSotDevice_;
public :
RCSotController ();
/// \brief Read the configuration files,
/// claims the request to the robot and initialize the Stack-Of-Tasks.
bool initRequest (lhi::RobotHW * robot_hw,
ros::NodeHandle &robot_nh,
ros::NodeHandle &controller_nh,
std::set<std::string> & claimed_resources);
/// \brief Display claimed resources
void displayClaimedResources(std::set<std::string> & claimed_resources);
/// \brief Claims
bool init();
/// \brief Read the sensor values, calls the control graph, and apply the control.
///
void update(const ros::Time&, const ros::Duration& );
/// \brief Starting by filling the sensors.
void starting(const ros::Time&);
/// \brief Stopping the control
void stopping(const ros::Time&);
/// \brief Display the kind of hardware interface that this controller is using.
virtual std::string getHardwareInterfaceType() const;
protected:
/// Initialize the roscontrol interfaces
bool initInterfaces(lhi::RobotHW * robot_hw,
ros::NodeHandle &,
ros::NodeHandle &,
std::set<std::string> & claimed_resources);
/// Initialize the hardware interface using the joints.
bool initJoints();
/// Initialize the hardware interface accessing the IMU.
bool initIMU();
/// Initialize the hardware interface accessing the force sensors.
bool initForceSensors();
/// Initialize the hardware interface accessing the temperature sensors.
bool initTemperatureSensors();
///@{ \name Read the parameter server
/// \brief Entry point
bool readParams(ros::NodeHandle &robot_nh);
/// \brief Creates the list of joint names.
bool readParamsJointNames(ros::NodeHandle &robot_nh);
/// \brief Set the SoT library name.
bool readParamsSotLibName(ros::NodeHandle &robot_nh);
/// \Brief Set the mapping between ros-control and the robot device
/// For instance the yaml file should have a line with map_rc_to_sot_device:
/// map_rc_to_sot_device: [ ]
bool readParamsFromRCToSotDevice(ros::NodeHandle &robot_nh);
/// \brief Read the control mode.
bool readParamsControlMode(ros::NodeHandle & robot_nh);
/// \brief Read the PID information of the robot in effort mode.
bool readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh);
///@}
/// \brief Fill the SoT map structures
void fillSensorsIn(std::string &title, std::vector<double> & data);
/// \brief Get the information from the low level and calls fillSensorsIn.
void fillJoints();
/// In the map sensorsIn_ creates the key "name_IMUNb"
/// and associate to this key the vector data.
void setSensorsImu(std::string &name,
int IMUNb,
std::vector<double> &data);
/// @{ \name Fill the sensors
/// Read the imus and set the interface to the SoT.
void fillImu();
/// Read the force sensors
void fillForceSensors();
/// Read the temperature sensors
void fillTempSensors();
/// Entry point for reading all the sensors .
void fillSensors();
///@}
/// Extract control values to send to the simulator.
void readControl(std::map<std::string,dgs::ControlValues> &controlValues);
/// Map of sensor readings
std::map <std::string,dgs::SensorValues> sensorsIn_;
/// Map of control values
std::map<std::string,dgs::ControlValues> controlValues_;
/// \brief Command send to motors
/// Depending on control_mode it can be either
/// position control or torque control.
std::vector<double> command_;
/// One iteration: read sensor, compute the control law, apply control.
void one_iteration();
};
}
#endif /* RC_SOT_CONTROLLER_H */
talos_roscontrol_sot_talos/CMakeLists.txt deleted 100644 → 0
View file @ 5f8a699d
cmake_minimum_required(VERSION 2.8.3)
project(talos_roscontrol_sot_talos)
find_package(PkgConfig REQUIRED)
set(bullet_FOUND 0)
pkg_check_modules(bullet REQUIRED bullet)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
dynamic_graph_bridge
control_msgs
sensor_msgs
realtime_tools
talos_controller_interface
)
include_directories(include ${bullet_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
link_directories(${bullet_LIBRARY_DIRS})
catkin_package()
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
)
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(roscontrol_sot_talos_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
foreach(dir config launch)
install(DIRECTORY ${dir}
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
endforeach()
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_roscontrol_sot_talos.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
talos_roscontrol_sot_talos/config/sot_talos_params.yaml deleted 100644 → 0
View file @ 5f8a699d
sot_controller:
libname: libsot-pyrene-controller.so
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,head_1_joint, head_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
]
map_rc_to_sot_device: { motor-angles: motor-angles ,
joint-angles: joint-angles, velocities: velocities,
torques: torques, cmd-positions: control, cmd-effort: control }
control_mode: POSITION
talos_roscontrol_sot_talos/config/sot_talos_params_gazebo.yaml deleted 100644 → 0
View file @ 5f8a699d
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: POSITION
\ No newline at end of file