Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
S
sot-talos-balance
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container Registry
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
loco-3d
sot-talos-balance
Commits
f9cd29b5
Commit
f9cd29b5
authored
5 years ago
by
Gabriele Buondonno
Browse files
Options
Downloads
Patches
Plain Diff
[distribute] CoP
parent
78cd90b4
No related branches found
Branches containing commit
No related tags found
Tags containing commit
No related merge requests found
Pipeline
#4554
failed
5 years ago
Stage: test
Changes
3
Pipelines
2
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
include/sot/talos_balance/distribute-wrench.hh
+8
-3
8 additions, 3 deletions
include/sot/talos_balance/distribute-wrench.hh
src/distribute-wrench.cpp
+25
-20
25 additions, 20 deletions
src/distribute-wrench.cpp
unittest/python/test_distribute.py
+19
-1
19 additions, 1 deletion
unittest/python/test_distribute.py
with
52 additions
and
24 deletions
include/sot/talos_balance/distribute-wrench.hh
+
8
−
3
View file @
f9cd29b5
...
...
@@ -76,9 +76,9 @@ namespace dynamicgraph {
DECLARE_SIGNAL_INNER
(
qp_computations
,
int
);
DECLARE_SIGNAL_OUT
(
wrenchLeft
,
dynamicgraph
::
Vector
);
//
DECLARE_SIGNAL_OUT(copLeft, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT
(
copLeft
,
dynamicgraph
::
Vector
);
DECLARE_SIGNAL_OUT
(
wrenchRight
,
dynamicgraph
::
Vector
);
//
DECLARE_SIGNAL_OUT(copRight, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT
(
copRight
,
dynamicgraph
::
Vector
);
DECLARE_SIGNAL_OUT
(
wrenchRef
,
dynamicgraph
::
Vector
);
DECLARE_SIGNAL_OUT
(
zmpRef
,
dynamicgraph
::
Vector
);
...
...
@@ -88,7 +88,7 @@ namespace dynamicgraph {
/* --- ENTITY INHERITANCE --- */
virtual
void
display
(
std
::
ostream
&
os
)
const
;
//
dynamicgraph::Vector computeCoP(const dynamicgraph::Vector & wrench, const
MatrixHomogeneous
& pose) const;
dynamicgraph
::
Vector
computeCoP
(
const
dynamicgraph
::
Vector
&
wrench
,
const
pinocchio
::
SE3
&
pose
)
const
;
protected:
bool
m_initSucceeded
;
/// true if the entity has been successfully initialized
...
...
@@ -96,6 +96,11 @@ namespace dynamicgraph {
pinocchio
::
Data
m_data
;
/// Pinocchio robot data
RobotUtilShrPtr
m_robot_util
;
pinocchio
::
SE3
m_ankle_M_ftSens
;
/// ankle to F/T sensor transformation
pinocchio
::
FrameIndex
m_left_foot_id
;
pinocchio
::
FrameIndex
m_right_foot_id
;
dynamicgraph
::
Vector
m_wrenchLeft
;
dynamicgraph
::
Vector
m_wrenchRight
;
...
...
This diff is collapsed.
Click to expand it.
src/distribute-wrench.cpp
+
25
−
20
View file @
f9cd29b5
...
...
@@ -16,6 +16,8 @@
#include
"sot/talos_balance/distribute-wrench.hh"
#include
<iostream>
#include
<sot/core/debug.hh>
#include
<dynamic-graph/factory.h>
#include
<dynamic-graph/command-bind.h>
...
...
@@ -45,8 +47,7 @@ namespace dynamicgraph
#define INNER_SIGNALS m_kinematics_computations << m_qp_computations
//#define OUTPUT_SIGNALS m_wrenchLeftSOUT << m_copLeftSOUT << m_wrenchRightSOUT << m_copRightSOUT << m_wrenchRefSOUT << m_zmpRefSOUT << m_emergencyStopSOUT
#define OUTPUT_SIGNALS m_wrenchLeftSOUT << m_wrenchRightSOUT << m_wrenchRefSOUT << m_zmpRefSOUT << m_emergencyStopSOUT
#define OUTPUT_SIGNALS m_wrenchLeftSOUT << m_copLeftSOUT << m_wrenchRightSOUT << m_copRightSOUT << m_wrenchRefSOUT << m_zmpRefSOUT << m_emergencyStopSOUT
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
...
...
@@ -66,9 +67,9 @@ namespace dynamicgraph
,
CONSTRUCT_SIGNAL_INNER
(
kinematics_computations
,
int
,
m_qSIN
)
,
CONSTRUCT_SIGNAL_INNER
(
qp_computations
,
int
,
m_wrenchDesSIN
<<
m_kinematics_computationsSINNER
)
,
CONSTRUCT_SIGNAL_OUT
(
wrenchLeft
,
dynamicgraph
::
Vector
,
m_qp_computationsSINNER
)
//
, CONSTRUCT_SIGNAL_OUT(copLeft, dynamicgraph::Vector, m_wrenchLeftSOUT)
,
CONSTRUCT_SIGNAL_OUT
(
copLeft
,
dynamicgraph
::
Vector
,
m_wrenchLeftSOUT
)
,
CONSTRUCT_SIGNAL_OUT
(
wrenchRight
,
dynamicgraph
::
Vector
,
m_qp_computationsSINNER
)
//
, CONSTRUCT_SIGNAL_OUT(copRight, dynamicgraph::Vector, m_wrenchRightSOUT)
,
CONSTRUCT_SIGNAL_OUT
(
copRight
,
dynamicgraph
::
Vector
,
m_wrenchRightSOUT
)
,
CONSTRUCT_SIGNAL_OUT
(
wrenchRef
,
dynamicgraph
::
Vector
,
m_wrenchLeftSOUT
<<
m_wrenchRightSOUT
)
,
CONSTRUCT_SIGNAL_OUT
(
zmpRef
,
dynamicgraph
::
Vector
,
m_wrenchRefSOUT
)
,
CONSTRUCT_SIGNAL_OUT
(
emergencyStop
,
bool
,
m_zmpRefSOUT
)
...
...
@@ -114,6 +115,14 @@ namespace dynamicgraph
return
;
}
assert
(
m_model
.
existFrame
(
m_robot_util
->
m_foot_util
.
m_Left_Foot_Frame_Name
));
assert
(
m_model
.
existFrame
(
m_robot_util
->
m_foot_util
.
m_Right_Foot_Frame_Name
));
m_left_foot_id
=
m_model
.
getFrameId
(
m_robot_util
->
m_foot_util
.
m_Left_Foot_Frame_Name
);
m_right_foot_id
=
m_model
.
getFrameId
(
m_robot_util
->
m_foot_util
.
m_Right_Foot_Frame_Name
);
m_ankle_M_ftSens
=
pinocchio
::
SE3
(
Eigen
::
Matrix3d
::
Identity
(),
m_robot_util
->
m_foot_util
.
m_Right_Foot_Force_Sensor_XYZ
.
head
<
3
>
());
// TODO: initialize m_qp1
m_qp2
.
problem
(
12
,
6
,
0
);
...
...
@@ -121,11 +130,12 @@ namespace dynamicgraph
m_initSucceeded
=
true
;
}
/*
dynamicgraph
::
Vector
DistributeWrench::computeCoP(const dg::Vector & wrench, const
MatrixHomogeneous
& pose) const
DistributeWrench
::
computeCoP
(
const
dg
::
Vector
&
wrench
Global
,
const
pinocchio
::
SE3
&
pose
)
const
{
const double h = pose(2,3);
dg
::
Vector
wrench
=
pose
.
actInv
(
pinocchio
::
Force
(
wrenchGlobal
)).
toVector
();
const
double
h
=
pose
.
translation
()[
2
];
const
double
fx
=
wrench
[
0
];
const
double
fy
=
wrench
[
1
];
...
...
@@ -136,6 +146,7 @@ namespace dynamicgraph
double
m_eps
=
0.1
;
// temporary
double
px
,
py
;
if
(
fz
>=
m_eps
/
2
)
{
px
=
(
-
ty
-
fx
*
h
)
/
fz
;
...
...
@@ -146,18 +157,15 @@ namespace dynamicgraph
px
=
0.0
;
py
=
0.0
;
}
const double pz = - h;
dg::Vector copLocal(3);
copLocal[0] = px;
copLocal[1] = py;
copLocal[2] = pz;
const
double
pz
=
0.0
;
dg::Vector cop = pose.linear()*copLocal + pose.translation();
dg
::
Vector
cop
(
3
);
cop
[
0
]
=
px
;
cop
[
1
]
=
py
;
cop
[
2
]
=
pz
;
return
cop
;
}
*/
/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
...
...
@@ -264,7 +272,6 @@ namespace dynamicgraph
return
s
;
}
/*
DEFINE_SIGNAL_OUT_FUNCTION
(
copLeft
,
dynamicgraph
::
Vector
)
{
if
(
!
m_initSucceeded
)
...
...
@@ -276,7 +283,7 @@ namespace dynamicgraph
const
Eigen
::
VectorXd
&
wrenchLeft
=
m_wrenchLeftSOUT
(
iter
);
// stub
const
MatrixHomogeneous & pose = MatrixHomogeneous::Identity()
;
const
pinocchio
::
SE3
pose
=
m_data
.
oMf
[
m_left_foot_id
]
*
m_ankle_M_ftSens
;
s
=
computeCoP
(
wrenchLeft
,
pose
);
...
...
@@ -293,14 +300,12 @@ namespace dynamicgraph
const
Eigen
::
VectorXd
&
wrenchRight
=
m_wrenchRightSOUT
(
iter
);
// stub
const MatrixHomogeneous & pose = MatrixHomogeneous::Identity();
const
pinocchio
::
SE3
pose
=
m_data
.
oMf
[
m_right_foot_id
]
*
m_ankle_M_ftSens
;
s
=
computeCoP
(
wrenchRight
,
pose
);
return
s
;
}
*/
DEFINE_SIGNAL_OUT_FUNCTION
(
wrenchRef
,
dynamicgraph
::
Vector
)
{
...
...
This diff is collapsed.
Click to expand it.
unittest/python/test_distribute.py
+
19
−
1
View file @
f9cd29b5
...
...
@@ -50,12 +50,14 @@ rightPos = data.oMf[rightId]
#print( "%s: %d" % (rightName,rightId) )
#print(rightPos)
print
(
"
wrenches in GLOBAL frame:
"
)
g
=
9.81
fz
=
m
*
g
force
=
[
0.0
,
0.0
,
fz
]
forceLeft
=
[
0.0
,
0.0
,
fz
/
2
]
forceRight
=
[
0.0
,
0.0
,
fz
/
2
]
lever
=
float
(
com
[
0
]
-
rightPos
.
translation
[
0
]
)
lever
=
float
(
com
[
0
])
tauy
=
-
fz
*
lever
wrench
=
force
+
[
0.0
,
tauy
,
0.0
]
wrenchLeft
=
forceLeft
+
[
0.0
,
tauy
/
2
,
0.0
]
...
...
@@ -65,6 +67,14 @@ print( "desired wrench: %s" % str(wrench) )
print
(
"
expected left wrench: %s
"
%
str
(
wrenchLeft
)
)
print
(
"
expected right wrench: %s
"
%
str
(
wrenchRight
)
)
print
(
"
CoP in LOCAL sole frame:
"
)
copLeft
=
[
float
(
com
[
0
]
-
leftPos
.
translation
[
0
]),
-
float
(
leftPos
.
translation
[
1
]),
0.
]
copRight
=
[
float
(
com
[
0
]
-
rightPos
.
translation
[
0
]),
-
float
(
rightPos
.
translation
[
1
]),
0.
]
print
(
"
expected left CoP: %s
"
%
str
(
copLeft
)
)
print
(
"
expected right CoP: %s
"
%
str
(
copRight
)
)
# --- Parameter server ---
print
(
"
--- Parameter server ---
"
)
...
...
@@ -89,6 +99,14 @@ assertApprox(wrenchLeft,distribute.wrenchLeft.value,6)
print
(
"
resulting right wrench: %s
"
%
str
(
distribute
.
wrenchRight
.
value
)
)
assertApprox
(
wrenchRight
,
distribute
.
wrenchRight
.
value
,
6
)
distribute
.
copLeft
.
recompute
(
0
)
distribute
.
copRight
.
recompute
(
0
)
print
(
"
resulting left CoP: %s
"
%
str
(
distribute
.
copLeft
.
value
)
)
assertApprox
(
copLeft
,
distribute
.
copLeft
.
value
,
3
)
print
(
"
resulting right CoP: %s
"
%
str
(
distribute
.
copRight
.
value
)
)
assertApprox
(
copRight
,
distribute
.
copRight
.
value
,
3
)
distribute
.
emergencyStop
.
recompute
(
0
)
stop
=
distribute
.
emergencyStop
.
value
np
.
testing
.
assert_equal
(
stop
,
0
)
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment