Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
S
sot-dynamic-pinocchio
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Issues
0
Issues
0
List
Boards
Labels
Service Desk
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Operations
Operations
Incidents
Environments
Packages & Registries
Packages & Registries
Container Registry
Analytics
Analytics
CI / CD
Repository
Value Stream
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Stack Of Tasks
sot-dynamic-pinocchio
Commits
520ed959
Commit
520ed959
authored
Jan 09, 2019
by
Olivier Stasse
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
[pinocchio-device] Checked unit test for one rotation joint.
parent
da269cd8
Pipeline
#2055
failed with stage
in 6 minutes and 27 seconds
Changes
3
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
166 additions
and
141 deletions
+166
-141
include/sot-dynamic-pinocchio/pinocchio-device.hh
include/sot-dynamic-pinocchio/pinocchio-device.hh
+4
-3
src/pinocchio-device.cpp
src/pinocchio-device.cpp
+62
-23
unitTesting/test_pinocchio_device.cpp
unitTesting/test_pinocchio_device.cpp
+100
-115
No files found.
include/sot-dynamic-pinocchio/pinocchio-device.hh
View file @
520ed959
...
...
@@ -131,7 +131,10 @@ namespace dynamicgraph {
return
CLASS_NAME
;
}
static
const
double
TIMESTEP_DEFAULT
;
/// Maps of joint devices.
std
::
map
<
std
::
string
,
dgsot
::
JointSoTHWControlType
>
jointPinocchioDevices_
;
protected:
/// \brief Current integration step.
double
timestep_
;
...
...
@@ -159,8 +162,6 @@ namespace dynamicgraph {
std
::
map
<
std
::
string
,
ControlType
>
sotControlType_
;
std
::
map
<
std
::
string
,
ControlType
>
hwControlType_
;
/// Maps of joint devices.
std
::
map
<
std
::
string
,
JointSoTHWControlType
>
jointPinocchioDevices_
;
///
PeriodicCall
periodicCallBefore_
;
PeriodicCall
periodicCallAfter_
;
...
...
src/pinocchio-device.cpp
View file @
520ed959
...
...
@@ -29,7 +29,7 @@ using namespace std;
using
namespace
dynamicgraph
::
sot
;
using
namespace
dynamicgraph
;
#define DBGFILE "/tmp/
sot-talos
-device.txt"
#define DBGFILE "/tmp/
pinocchio
-device.txt"
#if 0
#define RESETDEBUG5() { std::ofstream DebugFile; \
...
...
@@ -295,8 +295,17 @@ setURDFModel(const std::string &aURDFModel)
/// Build the map between pinocchio and the alphabetical order.
for
(
unsigned
int
i
=
0
;
i
<
model_
.
names
.
size
();
i
++
)
jointPinocchioDevices_
[
model_
.
names
[
i
]].
pinocchio_index
=
i
;
{
if
(
model_
.
joints
[
i
].
id
()
<
model_
.
nq
)
{
jointPinocchioDevices_
[
model_
.
names
[
i
]].
pinocchio_index
=
i
;
std
::
cout
<<
"jointPinocchioDevices_ index: "
<<
i
<<
" model_.joints[i].id(): "
<<
model_
.
joints
[
i
].
id
()
<<
" model_.names[i]: "
<<
model_
.
names
[
i
]
<<
" model_.joints[i].shortname() : "
<<
model_
.
joints
[
i
].
shortname
()
<<
std
::
endl
;
}
}
// Initialize pinocchio vector.
position_
.
resize
(
model_
.
nq
);
velocity_
.
resize
(
model_
.
nv
);
...
...
@@ -400,6 +409,14 @@ void PinocchioDevice::integrate( const double & dt )
int
lctl_index
=
it_control_type
->
second
.
control_index
;
int
pino_index
=
it_control_type
->
second
.
pinocchio_index
;
if
(
lctl_index
==-
1
)
{
if
(
debug_mode_
>
1
)
std
::
cerr
<<
"No control index for joint "
<<
model_
.
names
[
pino_index
]
<<
std
::
endl
;
break
;
}
if
(
pino_index
!=-
1
)
{
int
lpos_index
=
model_
.
joints
[
pino_index
].
idx_q
();
...
...
@@ -410,6 +427,8 @@ void PinocchioDevice::integrate( const double & dt )
double
lvelocityLimit
=
model_
.
velocityLimit
[
lvel_index
];
// Integration.
// Set acceleration from control and integrates to find velocity.
if
(
it_control_type
->
second
.
SoTcontrol
==
ACCELERATION
)
{
acceleration_
[
lvel_index
]
=
controlIN
[
lctl_index
];
...
...
@@ -423,6 +442,7 @@ void PinocchioDevice::integrate( const double & dt )
shouldIntegrateVelocity
=
true
;
}
// Set velocity from control and set boolean to perform velocity integration later on.
else
if
(
it_control_type
->
second
.
SoTcontrol
==
VELOCITY
)
{
acceleration_
[
lvel_index
]
=
0
;
...
...
@@ -434,6 +454,7 @@ void PinocchioDevice::integrate( const double & dt )
shouldIntegrateVelocity
=
true
;
}
// Position from control is directly provided.
else
if
(
it_control_type
->
second
.
SoTcontrol
==
POSITION
)
{
acceleration_
[
lvel_index
]
=
0
;
...
...
@@ -509,6 +530,7 @@ ParseYAMLString(const std::string & aYamlString)
if
(
r
<
0
)
return
r
;
}
}
UpdateSignals
();
return
0
;
}
...
...
@@ -720,20 +742,25 @@ void PinocchioDevice::CreateAnImuSignal(const std::string &imu_sensor_name)
int
PinocchioDevice
::
UpdateSignals
()
{
pseudoTorqueSOUT_
=
new
Signal
<
Vector
,
int
>
(
"PinocchioDevice("
+
getName
()
+
")::output(vector)::ptorque"
);
currentsSOUT_
=
new
Signal
<
Vector
,
int
>
(
"PinocchioDevice("
+
getName
()
+
")::output(vector)::currents"
);
if
((
torque_index_
!=
0
)
&&
(
pseudoTorqueSOUT_
!=
0
))
pseudoTorqueSOUT_
=
new
Signal
<
Vector
,
int
>
(
"PinocchioDevice("
+
getName
()
+
")::output(vector)::ptorque"
);
temperatureSOUT_
=
new
Signal
<
Vector
,
int
>
(
"PinocchioDevice("
+
getName
()
+
")::output(vector)::temperatures"
);
if
((
current_index_
!=
0
)
&&
(
currentsSOUT_
!=
0
))
currentsSOUT_
=
new
Signal
<
Vector
,
int
>
(
"PinocchioDevice("
+
getName
()
+
")::output(vector)::currents"
);
motor_anglesSOUT_
=
new
Signal
<
Vector
,
int
>
(
"PinocchioDevice("
+
getName
()
+
")::output(vector)::motor_angles"
);
if
((
temperature_index_
!=
0
)
&&
(
temperatureSOUT_
!=
0
))
temperatureSOUT_
=
new
Signal
<
Vector
,
int
>
(
"PinocchioDevice("
+
getName
()
+
")::output(vector)::temperatures"
);
joint_anglesSOUT_
=
new
Signal
<
Vector
,
int
>
(
"PinocchioDevice("
+
getName
()
+
")::output(vector)::joint_angles"
);
if
((
motor_angle_index_
!=
0
)
&&
(
motor_anglesSOUT_
!=
0
))
motor_anglesSOUT_
=
new
Signal
<
Vector
,
int
>
(
"PinocchioDevice("
+
getName
()
+
")::output(vector)::motor_angles"
);
if
((
joint_angle_index_
!=
0
)
&&
(
joint_anglesSOUT_
!=
0
))
joint_anglesSOUT_
=
new
Signal
<
Vector
,
int
>
(
"PinocchioDevice("
+
getName
()
+
")::output(vector)::joint_angles"
);
return
0
;
}
...
...
@@ -1000,8 +1027,9 @@ void PinocchioDevice::getControl(map<string,dgsot::ControlValues> &controlOut)
{
ODEBUG5FULL
(
"start"
);
sotDEBUGIN
(
25
)
;
vector
<
double
>
anglesOut
;
anglesOut
.
resize
(
state_
.
size
());
const
Vector
&
controlIN
=
controlSIN
.
accessCopy
();
vector
<
double
>
lcontrolOut
;
lcontrolOut
.
resize
(
controlIN
.
size
());
// Integrate control
increment
(
timestep_
);
...
...
@@ -1010,13 +1038,24 @@ void PinocchioDevice::getControl(map<string,dgsot::ControlValues> &controlOut)
ODEBUG5FULL
(
"state = "
<<
state_
);
// Specify the joint values for the controller.
if
((
int
)
anglesOut
.
size
()
!=
state_
.
size
()
-
6
)
anglesOut
.
resize
(
state_
.
size
()
-
6
);
for
(
unsigned
int
i
=
6
;
i
<
state_
.
size
();
++
i
)
anglesOut
[
i
-
6
]
=
state_
(
i
);
controlOut
[
"control"
].
setValues
(
anglesOut
);
JointSHWControlType_iterator
it_control_type
;
for
(
it_control_type
=
jointPinocchioDevices_
.
begin
();
it_control_type
!=
jointPinocchioDevices_
.
end
();
it_control_type
++
)
{
int
lctl_index
=
it_control_type
->
second
.
control_index
;
if
(
it_control_type
->
second
.
HWcontrol
==
TORQUE
)
lcontrolOut
[
lctl_index
]
=
controlIN
[
lctl_index
];
else
if
(
it_control_type
->
second
.
HWcontrol
==
POSITION
)
{
int
pino_index
=
it_control_type
->
second
.
pinocchio_index
;
int
lpos_index
=
model_
.
joints
[
pino_index
].
idx_q
();
lcontrolOut
[
lctl_index
]
=
position_
[
lpos_index
];
}
}
controlOut
[
"control"
].
setValues
(
lcontrolOut
);
ODEBUG5FULL
(
"end"
);
sotDEBUGOUT
(
25
)
;
}
...
...
unitTesting/test_pinocchio_device.cpp
View file @
520ed959
...
...
@@ -29,220 +29,188 @@ void CreateYAMLFILE()
{
YAML
::
Emitter
yaml_out
;
YAML
::
Node
aNode
,
yn_map_hw_sot_c
,
yn_map_sensors
;
unsigned
int
index_vec_ctl
=
0
;
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"]["controlPos"] =
0
;
yn_map_hw_sot_c["waist"]["sensors"] = "";
index_vec_ctl
+=
6
;
*/
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"
]
=
6
;
yn_map_hw_sot_c
[
"LLEG_HIP_P"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
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"
]
=
7
;
yn_map_hw_sot_c
[
"LLEG_HIP_R"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
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"
]
=
8
;
yn_map_hw_sot_c
[
"LLEG_HIP_Y"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
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"
]
=
9
;
yn_map_hw_sot_c
[
"LLEG_KNEE"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
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"
]
=
10
;
yn_map_hw_sot_c
[
"LLEG_ANKLE_P"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
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"
]
=
11
;
yn_map_hw_sot_c
[
"LLEG_ANKLE_R"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
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"
][
"controlPos"
]
=
12
;
yn_map_hw_sot_c
[
"RLEG_HIP_P"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
13
;
yn_map_hw_sot_c
[
"RLEG_HIP_R"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
14
;
yn_map_hw_sot_c
[
"RLEG_HIP_Y"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
15
;
yn_map_hw_sot_c
[
"RLEG_KNEE"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
16
;
yn_map_hw_sot_c
[
"RLEG_ANKLE_P"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
17
;
yn_map_hw_sot_c
[
"RLEG_ANKLE_R"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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
;
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
;
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
;
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
;
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
;
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"
;
yn_map_hw_sot_c
[
"WAIST_P"
][
"controlPos"
]
=
18
;
yn_map_hw_sot_c
[
"WAIST_P"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
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
;
yn_map_hw_sot_c
[
"WAIST_R"
];
yn_map_hw_sot_c
[
"WAIST_R"
][
"hw"
]
=
"POSITION"
;
yn_map_hw_sot_c
[
"WAIST_R"
][
"sot"
]
=
"VELOCITY"
;
yn_map_hw_sot_c
[
"WAIST_R"
][
"controlPos"
]
=
19
;
yn_map_hw_sot_c
[
"WAIST_R"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
yn_map_hw_sot_c
[
"CHEST"
];
yn_map_hw_sot_c
[
"CHEST"
][
"hw"
]
=
"POSITION"
;
yn_map_hw_sot_c
[
"CHEST"
][
"sot"
]
=
"VELOCITY"
;
yn_map_hw_sot_c
[
"CHEST"
][
"controlPos"
]
=
20
;
yn_map_hw_sot_c
[
"CHEST"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
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"
][
"controlPos"
]
=
21
;
yn_map_hw_sot_c
[
"RARM_SHOULDER_P"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
22
;
yn_map_hw_sot_c
[
"RARM_SHOULDER_R"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
23
;
yn_map_hw_sot_c
[
"RARM_SHOULDER_Y"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
24
;
yn_map_hw_sot_c
[
"RARM_ELBOW"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
25
;
yn_map_hw_sot_c
[
"RARM_WRIST_Y"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
26
;
yn_map_hw_sot_c
[
"RARM_WRIST_P"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
27
;
yn_map_hw_sot_c
[
"RARM_WRIST_R"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
28
;
yn_map_hw_sot_c
[
"LARM_SHOULDER_P"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
29
;
yn_map_hw_sot_c
[
"LARM_SHOULDER_R"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
30
;
yn_map_hw_sot_c
[
"LARM_SHOULDER_Y"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
31
;
yn_map_hw_sot_c
[
"LARM_ELBOW"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
32
;
yn_map_hw_sot_c
[
"LARM_WRIST_Y"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
33
;
yn_map_hw_sot_c
[
"LARM_WRIST_P"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
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"
][
"controlPos"
]
=
34
;
yn_map_hw_sot_c
[
"LARM_WRIST_R"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current]"
;
index_vec_ctl
+=
1
;
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"
;
yn_map_hw_sot_c
[
"WAIST_P"
][
"controlPos"
]
=
index_vec_ctl
;
yn_map_hw_sot_c
[
"WAIST_P"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
yn_map_hw_sot_c
[
"WAIST_R"
];
yn_map_hw_sot_c
[
"WAIST_R"
][
"hw"
]
=
"POSITION"
;
yn_map_hw_sot_c
[
"WAIST_R"
][
"sot"
]
=
"VELOCITY"
;
yn_map_hw_sot_c
[
"WAIST_R"
][
"controlPos"
]
=
index_vec_ctl
;
yn_map_hw_sot_c
[
"WAIST_R"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
yn_map_hw_sot_c
[
"CHEST"
];
yn_map_hw_sot_c
[
"CHEST"
][
"hw"
]
=
"POSITION"
;
yn_map_hw_sot_c
[
"CHEST"
][
"sot"
]
=
"VELOCITY"
;
yn_map_hw_sot_c
[
"CHEST"
][
"controlPos"
]
=
index_vec_ctl
;
yn_map_hw_sot_c
[
"CHEST"
][
"sensors"
]
=
"[motor_angle,joint_angle,temperature,current,torque]"
;
index_vec_ctl
+=
1
;
yn_map_sensors
[
"force_torque"
];
yn_map_sensors
[
"force_torque"
][
"left_ankle_ft"
];
...
...
@@ -353,7 +321,7 @@ int ReadYAMLFILE(dg::sot::PinocchioDevice &aDevice, unsigned int debug_mode)
namespace
dg
=
dynamicgraph
;
int
main
(
int
,
char
**
)
{
unsigned
int
debug_mode
=
0
;
unsigned
int
debug_mode
=
5
;
std
::
string
robot_description
;
ifstream
urdfFile
;
...
...
@@ -395,16 +363,33 @@ int main(int, char **)
const
se3
::
Model
&
aModel
=
aDevice
.
getModel
();
const
dg
::
Vector
&
aPosition
=
aDevice
.
stateSOUT_
(
2001
);
double
diff
=
0
;
for
(
unsigned
int
j
=
0
;
j
<
aPosition
.
size
();
j
++
)
double
diff
=
0
,
ldiff
;
dgsot
::
JointSHWControlType_iterator
it_control_type
;
for
(
it_control_type
=
aDevice
.
jointPinocchioDevices_
.
begin
();
it_control_type
!=
aDevice
.
jointPinocchioDevices_
.
end
();
it_control_type
++
)
{
diff
+=
fabs
(
aPosition
(
j
)
-
aModel
.
lowerPositionLimit
[
j
]);
if
(
debug_mode
>
1
)
std
::
cout
<<
diff
<<
" "
<<
aModel
.
names
[
j
]
<<
" "
<<
aPosition
(
j
)
<<
" "
<<
aModel
.
lowerPositionLimit
[
j
]
<<
std
::
endl
;
int
lctl_index
=
it_control_type
->
second
.
control_index
;
if
(
it_control_type
->
second
.
HWcontrol
==
dgsot
::
POSITION
)
{
int
pino_index
=
it_control_type
->
second
.
pinocchio_index
;
if
(
pino_index
!=-
1
)
{
int
lpos_index
=
aModel
.
joints
[
pino_index
].
idx_q
();
int
lvel_index
=
aModel
.
joints
[
pino_index
].
idx_v
();
ldiff
=
(
aPosition
[
lpos_index
]
-
aModel
.
lowerPositionLimit
[
lpos_index
]);
diff
+=
ldiff
;
std
::
cout
<<
ldiff
<<
" "
<<
aModel
.
names
[
pino_index
]
<<
" "
<<
aPosition
[
lpos_index
]
<<
" "
<<
aModel
.
lowerPositionLimit
[
lpos_index
]
<<
" "
<<
-
aModel
.
velocityLimit
[
lvel_index
]
<<
std
::
endl
;
}
}
}
if
(
diff
>
1e-3
)
return
-
1
;
return
1
;
return
0
;
}
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment