Skip to content

GitLab

Commit af1bad3f authored by Olivier Stasse's avatar Olivier Stasse
Browse files

pinocchio-device: Add YAML parsing to make the map with the sensors. 

WIP.
parent a6224adb
Pipeline #2042 failed with stage
in 4 minutes and 36 seconds
Expand all Hide whitespace changes
Inline Side-by-side
include/sot-dynamic-pinocchio/pinocchio-device.hh
View file @ af1bad3f
......@@ -15,22 +15,33 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/// URDF DOM
#include <urdf_parser/urdf_parser.h>
/// YAML CPP
#include <yaml-cpp/yaml.h>
/* -- MaaL --- */
#include <dynamic-graph/linear-algebra.h>
namespace dg = dynamicgraph;
/* SOT */
/// dg
#include <dynamic-graph/entity.h>
#include <dynamic-graph/all-signals.h>
/// sot-core
#include "sot/core/periodic-call.hh"
#include <sot/core/matrix-geometry.hh>
#include "sot/core/api.hh"
#include <sot/core/abstract-sot-external-interface.hh>
/// pinocchio
#include <pinocchio/multibody/liegroup/special-euclidean.hpp>
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp"
#include "pinocchio/math/quaternion.hpp"
namespace dgsot=dynamicgraph::sot;
namespace dg=dynamicgraph;
namespace dynamicgraph {
namespace sot {
......@@ -51,17 +62,51 @@ namespace dynamicgraph {
//@}
/// \brief Store information on each joint.
struct JointSoTHWControlType
{
/// Defines the control from the Stack-of-Tasks side (for instance, velocity)
ControlType SoTcontrol;
/// Defines the control from the hardware side.
ControlType HWcontrol;
/// Position of the joint in the control vector.
int control_index;
/// Position of the joint in the pinocchio/URDF index.
int pinocchio_index;
/// Various indexes for the sensor signals.
/// This may vary if some joints does not support this feature.
///@{
/// Position of the joint in the temperature vector
int temperature_index;
/// Position of the joint in the velocity vector
int velocity_index;
/// Position of the joint in the current vector
int current_index;
/// Position of the joint in the torque vector
int torque_index;
/// Position of the joint in the joint-angles vector
int joint_angle_index;
/// Position of the joint in the motor-angles vector
int motor_angle_index;
///@}
JointSoTHWControlType();
};
struct IMUSOUT
{
dg::Signal<MatrixRotation, int> attitudeSOUT;
dg::Signal<dg::Vector,int> accelerometerSOUT;
dg::Signal<dg::Vector,int> gyrometerSOUT;
};
typedef std::map<std::string,JointSoTHWControlType>::iterator
JointSHControlType_iterator;
JointSHWControlType_iterator;
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
......@@ -75,14 +120,6 @@ namespace dynamicgraph {
return CLASS_NAME;
}
enum ForceSignalSource
{
FORCE_SIGNAL_RLEG,
FORCE_SIGNAL_LLEG,
FORCE_SIGNAL_RARM,
FORCE_SIGNAL_LARM
};
protected:
/// \name Vectors related to the state.
......@@ -111,7 +148,6 @@ namespace dynamicgraph {
/// Maps of joint devices.
std::map<std::string,JointSoTHWControlType> jointPinocchioDevices_;
///
bool withForceSignals[4];
PeriodicCall periodicCallBefore_;
PeriodicCall periodicCallAfter_;
......@@ -158,32 +194,19 @@ namespace dynamicgraph {
/// This entity needs a control vector to be send to the hardware.
/// The control vector can be position, velocity and effort.
/// It depends on each of the actuator
dynamicgraph::SignalPtr<dg::Vector,int> controlSIN;
/// \name This part is specific to robot where a stabilizer is provided outside the
/// SoT framework and needs input.
/// @{
/// Input signal handling the attitude of the freeflyer.
dynamicgraph::SignalPtr<dg::Vector,int> attitudeSIN;
/// Input signal handling the ZMP of the system
dynamicgraph::SignalPtr<dg::Vector,int> zmpSIN;
///@}
dg::SignalPtr<dg::Vector,int> controlSIN;
/// \name PinocchioDevice current state.
/// \{
/// \brief Output integrated state from control.
dynamicgraph::Signal<dg::Vector,int> stateSOUT;
dg::Signal<dg::Vector,int> stateSOUT_;
/// \brief Output integrated velocity from control
dynamicgraph::Signal<dg::Vector,int> velocitySOUT;
dg::Signal<dg::Vector,int> *velocitySOUT_;
/// \brief Output attitude provided by the hardware
/// Typically this can be provided by an external estimator
/// such an integrated/hardware implemented EKF.
dynamicgraph::Signal<MatrixRotation,int> attitudeSOUT;
/*! \brief The current state of the robot from the command viewpoint. */
dynamicgraph::Signal<dg::Vector,int> motorcontrolSOUT;
dynamicgraph::Signal<dg::Vector,int> previousControlSOUT;
/*! \brief The ZMP reference send by the previous controller. */
dynamicgraph::Signal<dg::Vector,int> ZMPPreviousControllerSOUT;
dg::Signal<dg::Vector,int> motorcontrolSOUT_;
dg::Signal<dg::Vector,int> previousControlSOUT_;
/// \}
/// \name Real robot current state
......@@ -192,16 +215,41 @@ namespace dynamicgraph {
/// does *not* match the state control input signal.
/// \{
/// Motor positions
dynamicgraph::Signal<dg::Vector, int> robotState_;
dg::Signal<dg::Vector, int> robotState_;
/// Motor velocities
dynamicgraph::Signal<dg::Vector, int> robotVelocity_;
dg::Signal<dg::Vector, int> robotVelocity_;
/// The force torque sensors
dynamicgraph::Signal<dg::Vector,int>* forcesSOUT[4];
/// Motor torques
/// \todo why pseudo ?
dynamicgraph::Signal<dg::Vector,int> pseudoTorqueSOUT;
std::vector<dg::Signal<dg::Vector,int>*> forcesSOUT_;
/// The imu sensors
std::vector<dg::Signal<dg::Vector, int>*> imuSOUT_;
/// Motor or pseudo torques (estimated or build from PID)
dg::Signal<dg::Vector,int> * pseudoTorqueSOUT_;
/// Temperature signal
dg::Signal<dg::Vector,int> * temperatureSOUT_;
/// Current signal
dg::Signal<dg::Vector,int> * currentsSOUT_;
/// Motor angles signal;
dg::Signal<dg::Vector, int> * motor_anglesSOUT_;
/// Joint angles signal;
dg::Signal<dg::Vector, int> * joint_anglesSOUT_;
/// \}
/// Parse a YAML string for configuration.
int ParseYAMLString(const std::string &aYamlString);
/// \name Robot Side
///@{
/// \brief Allows the robot to fill in the internal variables of the device
/// to publish data on the signals.
void setSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);
void setupSetSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);
void nominalSetSensors(std::map<std::string,dgsot::SensorValues> &sensorsIn);
void cleanupSetSensors(std::map<std::string, dgsot::SensorValues> &sensorsIn);
/// \brief Provides to the robot the control information.
void getControl(std::map<std::string, dgsot::ControlValues> &anglesOut);
///@}
protected:
void setControlType(const std::string &strCtrlType,
ControlType &aCtrlType);
......@@ -211,6 +259,7 @@ namespace dynamicgraph {
double dt);
/// Store Position of free flyer joint
MatrixHomogeneous ffPose_;
/// Compute the new position, from the current control.
///
/// When sanity checks are enabled, this checks that the control is within
......@@ -224,13 +273,46 @@ namespace dynamicgraph {
/// pinocchio and the contact forces in order to estimate
/// the joint torques for the given acceleration.
virtual void integrate( const double & dt );
protected:
/// \name YAML related methods
/// @{
/// Parse YAML for mapping between hardware and sot
/// starting from a YAML-cpp node.
int ParseYAMLMapHardware(YAML::Node &map_hs_control);
/// Parse YAML for sensors from a YAML-cpp node.
int ParseYAMLSensors(YAML::Node &map_sensors);
/// Parse YAML for joint sencors from YAML-cpp node.
int ParseYAMLJointSensor(std::string & joint_name,
YAML::Node &aJointSensors);
/// @}
/// \name Signals related methods
///@{
/// \brief Creates a signal called PinocchioDevice(DeviceName)::output(vector6)::force_sensor_name
void CreateAForceSignal(const std::string & force_sensor_name);
/// \brief Creates a signal called PinocchioDevice(DeviceName)::output(vector6)::imu_sensor_name
void CreateAnImuSignal(const std::string & imu_sensor_name);
/// \brief Creates signals based on the joints information parsed by the YAML string.
int UpdateSignals();
///@}
/// Get freeflyer pose
const MatrixHomogeneous& freeFlyerPose() const;
public:
virtual void setRoot( const dg::Matrix & root );
/// Protected methods for internal variables filling
void setSensorsForce(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsIMU(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsEncoders(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsVelocities(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsTorquesCurrents(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
void setSensorsGains(std::map<std::string,dgsot::SensorValues> &SensorsIn, int t);
public:
virtual void setRoot( const dg::Matrix & root );
virtual void setRoot( const MatrixHomogeneous & worldMwaist );
private:
......@@ -240,6 +322,15 @@ namespace dynamicgraph {
// Pinocchio Model of the robot
se3::Model model_;
// Debug mode
int debug_mode_;
// Intermediate index when parsing YAML file.
int temperature_index_,velocity_index_,
current_index_,torque_index_,
joint_angles_index_,
motor_angles_index_
;
public:
const se3::Model & getModel()
{ return model_;}
......
src/pinocchio-device.cpp
View file @ af1bad3f
This diff is collapsed.
unitTesting/test_pinocchio_device.cpp
View file @ af1bad3f
......@@ -28,190 +28,248 @@ using namespace std;
void CreateYAMLFILE()
{
YAML::Emitter yaml_out;
YAML::Node aNode;
YAML::Node aNode,yn_map_hw_sot_cm,yn_map_sensors;
unsigned int index_vec_ctl=0;
aNode["map_hardware_sot_control"];
aNode["map_hardware_sot_control"]["waist"];
aNode["map_hardware_sot_control"]["waist"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["waist"]["sot"] = "POSITION";
aNode["map_hardware_sot_control"]["waist"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c = aNode["map_hardware_sot_control"];
yn_map_sensors = aNode["sensors"];
yn_map_hw_sot_c["waist"];
yn_map_hw_sot_c["waist"]["hw"] = "POSITION";
yn_map_hw_sot_c["waist"]["sot"] = "POSITION";
yn_map_hw_sot_c["waist"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["waist"]["sensors"] = "";
index_vec_ctl+=6;
aNode["map_hardware_sot_control"]["RLEG_HIP_P"];
aNode["map_hardware_sot_control"]["RLEG_HIP_P"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["RLEG_HIP_P"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["RLEG_HIP_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_HIP_P"];
yn_map_hw_sot_c["RLEG_HIP_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["RLEG_HIP_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RLEG_HIP_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_HIP_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["RLEG_HIP_R"];
aNode["map_hardware_sot_control"]["RLEG_HIP_R"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["RLEG_HIP_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RLEG_HIP_R"];
yn_map_hw_sot_c["RLEG_HIP_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["RLEG_HIP_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RLEG_HIP_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["RLEG_HIP_Y"];
aNode["map_hardware_sot_control"]["RLEG_HIP_Y"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["RLEG_HIP_Y"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["RLEG_HIP_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_HIP_Y"];
yn_map_hw_sot_c["RLEG_HIP_Y"]["hw"] = "POSITION";
yn_map_hw_sot_c["RLEG_HIP_Y"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RLEG_HIP_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_HIP_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["RLEG_KNEE"];
aNode["map_hardware_sot_control"]["RLEG_KNEE"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["RLEG_KNEE"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["RLEG_KNEE"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_KNEE"];
yn_map_hw_sot_c["RLEG_KNEE"]["hw"] = "POSITION";
yn_map_hw_sot_c["RLEG_KNEE"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RLEG_KNEE"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_KNEE"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["RLEG_ANKLE_P"];
aNode["map_hardware_sot_control"]["RLEG_ANKLE_P"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["RLEG_ANKLE_P"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["RLEG_ANKLE_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_ANKLE_P"];
yn_map_hw_sot_c["RLEG_ANKLE_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["RLEG_ANKLE_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RLEG_ANKLE_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_ANKLE_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["RLEG_ANKLE_R"];
aNode["map_hardware_sot_control"]["RLEG_ANKLE_R"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["RLEG_ANKLE_R"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["RLEG_ANKLE_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_ANKLE_R"];
yn_map_hw_sot_c["RLEG_ANKLE_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["RLEG_ANKLE_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RLEG_ANKLE_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RLEG_ANKLE_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["LLEG_HIP_P"];
aNode["map_hardware_sot_control"]["LLEG_HIP_P"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["LLEG_HIP_P"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["LLEG_HIP_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_HIP_P"];
yn_map_hw_sot_c["LLEG_HIP_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_HIP_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_HIP_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_HIP_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["LLEG_HIP_R"];
aNode["map_hardware_sot_control"]["LLEG_HIP_R"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["LLEG_HIP_R"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["LLEG_HIP_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_HIP_R"];
yn_map_hw_sot_c["LLEG_HIP_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_HIP_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_HIP_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_HIP_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["LLEG_HIP_Y"];
aNode["map_hardware_sot_control"]["LLEG_HIP_Y"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["LLEG_HIP_Y"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["LLEG_HIP_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_HIP_Y"];
yn_map_hw_sot_c["LLEG_HIP_Y"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_HIP_Y"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_HIP_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_HIP_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["LLEG_KNEE"];
aNode["map_hardware_sot_control"]["LLEG_KNEE"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["LLEG_KNEE"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["LLEG_KNEE"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_KNEE"];
yn_map_hw_sot_c["LLEG_KNEE"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_KNEE"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_KNEE"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_KNEE"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["LLEG_ANKLE_P"];
aNode["map_hardware_sot_control"]["LLEG_ANKLE_P"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["LLEG_ANKLE_P"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["LLEG_ANKLE_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_ANKLE_P"];
yn_map_hw_sot_c["LLEG_ANKLE_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_ANKLE_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_ANKLE_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_ANKLE_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["LLEG_ANKLE_R"];
aNode["map_hardware_sot_control"]["LLEG_ANKLE_R"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["LLEG_ANKLE_R"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["LLEG_ANKLE_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_ANKLE_R"];
yn_map_hw_sot_c["LLEG_ANKLE_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["LLEG_ANKLE_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LLEG_ANKLE_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LLEG_ANKLE_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["RARM_SHOULDER_P"];
aNode["map_hardware_sot_control"]["RARM_SHOULDER_P"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["RARM_SHOULDER_P"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["RARM_SHOULDER_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_SHOULDER_P"];
yn_map_hw_sot_c["RARM_SHOULDER_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["RARM_SHOULDER_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RARM_SHOULDER_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_SHOULDER_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["RARM_SHOULDER_R"];
aNode["map_hardware_sot_control"]["RARM_SHOULDER_R"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["RARM_SHOULDER_R"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["RARM_SHOULDER_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_SHOULDER_R"];
yn_map_hw_sot_c["RARM_SHOULDER_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["RARM_SHOULDER_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RARM_SHOULDER_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_SHOULDER_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["RARM_SHOULDER_Y"];
aNode["map_hardware_sot_control"]["RARM_SHOULDER_Y"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["RARM_SHOULDER_Y"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["RARM_SHOULDER_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_SHOULDER_Y"];
yn_map_hw_sot_c["RARM_SHOULDER_Y"]["hw"] = "POSITION";
yn_map_hw_sot_c["RARM_SHOULDER_Y"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RARM_SHOULDER_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_SHOULDER_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["RARM_ELBOW"];
aNode["map_hardware_sot_control"]["RARM_ELBOW"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["RARM_ELBOW"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["RARM_ELBOW"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_ELBOW"];
yn_map_hw_sot_c["RARM_ELBOW"]["hw"] = "POSITION";
yn_map_hw_sot_c["RARM_ELBOW"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RARM_ELBOW"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_ELBOW"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["RARM_WRIST_Y"];
aNode["map_hardware_sot_control"]["RARM_WRIST_Y"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["RARM_WRIST_Y"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["RARM_WRIST_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_WRIST_Y"];
yn_map_hw_sot_c["RARM_WRIST_Y"]["hw"] = "POSITION";
yn_map_hw_sot_c["RARM_WRIST_Y"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RARM_WRIST_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_WRIST_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["RARM_WRIST_P"];
aNode["map_hardware_sot_control"]["RARM_WRIST_P"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["RARM_WRIST_P"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["RARM_WRIST_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_WRIST_P"];
yn_map_hw_sot_c["RARM_WRIST_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["RARM_WRIST_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RARM_WRIST_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_WRIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["RARM_WRIST_R"];
aNode["map_hardware_sot_control"]["RARM_WRIST_R"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["RARM_WRIST_R"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["RARM_WRIST_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_WRIST_R"];
yn_map_hw_sot_c["RARM_WRIST_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["RARM_WRIST_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["RARM_WRIST_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["RARM_WRIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["LARM_SHOULDER_P"];
aNode["map_hardware_sot_control"]["LARM_SHOULDER_P"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["LARM_SHOULDER_P"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["LARM_SHOULDER_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_SHOULDER_P"];
yn_map_hw_sot_c["LARM_SHOULDER_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["LARM_SHOULDER_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LARM_SHOULDER_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_SHOULDER_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["LARM_SHOULDER_R"];
aNode["map_hardware_sot_control"]["LARM_SHOULDER_R"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["LARM_SHOULDER_R"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["LARM_SHOULDER_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_SHOULDER_R"];
yn_map_hw_sot_c["LARM_SHOULDER_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["LARM_SHOULDER_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LARM_SHOULDER_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_SHOULDER_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["LARM_SHOULDER_Y"];
aNode["map_hardware_sot_control"]["LARM_SHOULDER_Y"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["LARM_SHOULDER_Y"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["LARM_SHOULDER_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_SHOULDER_Y"];
yn_map_hw_sot_c["LARM_SHOULDER_Y"]["hw"] = "POSITION";
yn_map_hw_sot_c["LARM_SHOULDER_Y"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LARM_SHOULDER_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_SHOULDER_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["LARM_ELBOW"];
aNode["map_hardware_sot_control"]["LARM_ELBOW"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["LARM_ELBOW"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["LARM_ELBOW"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_ELBOW"];
yn_map_hw_sot_c["LARM_ELBOW"]["hw"] = "POSITION";
yn_map_hw_sot_c["LARM_ELBOW"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LARM_ELBOW"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_ELBOW"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["LARM_WRIST_Y"];
aNode["map_hardware_sot_control"]["LARM_WRIST_Y"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["LARM_WRIST_Y"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["LARM_WRIST_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_WRIST_Y"];
yn_map_hw_sot_c["LARM_WRIST_Y"]["hw"] = "POSITION";
yn_map_hw_sot_c["LARM_WRIST_Y"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LARM_WRIST_Y"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_WRIST_Y"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["LARM_WRIST_P"];
aNode["map_hardware_sot_control"]["LARM_WRIST_P"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["LARM_WRIST_P"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["LARM_WRIST_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_WRIST_P"];
yn_map_hw_sot_c["LARM_WRIST_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["LARM_WRIST_P"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LARM_WRIST_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_WRIST_P"]["sensors"] = "[motor_angle,joint_angle,temperature,current,torque]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["LARM_WRIST_R"];
aNode["map_hardware_sot_control"]["LARM_WRIST_R"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["LARM_WRIST_R"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["LARM_WRIST_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_WRIST_R"];
yn_map_hw_sot_c["LARM_WRIST_R"]["hw"] = "POSITION";
yn_map_hw_sot_c["LARM_WRIST_R"]["sot"] = "VELOCITY";
yn_map_hw_sot_c["LARM_WRIST_R"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["LARM_WRIST_R"]["sensors"] = "[motor_angle,joint_angle,temperature,current]";
index_vec_ctl+=1;
aNode["map_hardware_sot_control"]["WAIST_P"];
aNode["map_hardware_sot_control"]["WAIST_P"]["hw"] = "POSITION";
aNode["map_hardware_sot_control"]["WAIST_P"]["sot"] = "VELOCITY";
aNode["map_hardware_sot_control"]["WAIST_P"]["controlPos"] = index_vec_ctl;
yn_map_hw_sot_c["WAIST_P"];
yn_map_hw_sot_c["WAIST_P"]["hw"] = "POSITION";
yn_map_hw_sot_c["WAIST_P"]["sot"] = "VELOCITY";