Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
sot-dynamic-pinocchio
Commits
af1bad3f
Commit
af1bad3f
authored
Jan 03, 2019
by
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
Changes
3
Pipelines
1
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
;
JointSH
W
ControlType_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.
d
ynamicgraph
::
Signal
<
dg
::
Vector
,
int
>
stateSOUT
;
d
g
::
Signal
<
dg
::
Vector
,
int
>
stateSOUT
_
;
/// \brief Output integrated velocity from control
d
ynamicgraph
::
Signal
<
dg
::
Vector
,
int
>
velocitySOUT
;
d
g
::
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
d
ynamicgraph
::
Signal
<
dg
::
Vector
,
int
>
robotState_
;
d
g
::
Signal
<
dg
::
Vector
,
int
>
robotState_
;
/// Motor velocities
d
ynamicgraph
::
Signal
<
dg
::
Vector
,
int
>
robotVelocity_
;
d
g
::
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.
Click to expand it.
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"
;