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
ecbb1600
Commit
ecbb1600
authored
Mar 04, 2019
by
Guilhem Saurel
Browse files
Merge branch 'devel' of github.com:stack-of-tasks/sot-dynamic-pinocchio into devel
parents
95c7a5a0
b06763f9
Pipeline
#3363
passed with stage
in 27 minutes and 25 seconds
Changes
2
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
src/angle-estimator.cpp
View file @
ecbb1600
...
...
@@ -340,7 +340,7 @@ compute_xff_dotSOUT( dynamicgraph::Vector& res,
const
dynamicgraph
::
Matrix
&
J
=
jacobianSIN
(
time
);
const
dynamicgraph
::
Vector
&
dq
=
qdotSIN
(
time
);
const
int
nr
=
J
.
rows
(),
nc
=
J
.
cols
()
-
6
;
const
Eigen
::
Index
nr
=
J
.
rows
(),
nc
=
J
.
cols
()
-
6
;
assert
(
nr
==
6
);
dynamicgraph
::
Matrix
Ja
(
nr
,
nc
);
dynamicgraph
::
Vector
dqa
(
nc
);
for
(
int
j
=
0
;
j
<
nc
;
++
j
)
...
...
@@ -367,7 +367,7 @@ compute_qdotSOUT( dynamicgraph::Vector& res,
assert
(
dx
.
size
()
==
6
);
const
int
nr
=
dq
.
size
();
const
Eigen
::
Index
nr
=
dq
.
size
();
res
.
resize
(
nr
);
res
=
dq
;
for
(
int
i
=
0
;
i
<
6
;
++
i
)
res
(
i
)
=
dx
(
i
);
...
...
src/sot-dynamic-pinocchio.cpp
View file @
ecbb1600
...
...
@@ -16,6 +16,7 @@
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/spatial/motion.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/centroidal.hpp>
...
...
@@ -715,7 +716,7 @@ dg::Vector& DynamicPinocchio::computeZmp( dg::Vector& res,const int& time )
int
&
DynamicPinocchio
::
computeJacobians
(
int
&
dummy
,
const
int
&
time
)
{
sotDEBUGIN
(
25
);
const
Eigen
::
VectorXd
&
q
=
pinocchioPosSINTERN
.
access
(
time
);
pinocchio
::
computeJacobians
(
*
m_model
,
*
m_data
,
q
);
pinocchio
::
computeJ
ointJ
acobians
(
*
m_model
,
*
m_data
,
q
);
sotDEBUG
(
25
)
<<
"Jacobians updated"
<<
std
::
endl
;
sotDEBUGOUT
(
25
);
return
dummy
;
...
...
@@ -785,7 +786,10 @@ computeGenericEndeffJacobian(const bool isFrame, const bool isLocal, const int j
//Computes Jacobian in end-eff coordinates.
if
(
isFrame
){
pinocchio
::
framesForwardKinematics
(
*
m_model
,
*
m_data
);
pinocchio
::
getFrameJacobian
(
*
m_model
,
*
m_data
,(
pinocchio
::
Model
::
Index
)
jointId
,
tmp
);
pinocchio
::
getFrameJacobian
(
*
m_model
,
*
m_data
,
(
pinocchio
::
Model
::
Index
)
jointId
,
pinocchio
::
LOCAL
,
tmp
);
sotDEBUG
(
25
)
<<
"EndEffJacobian for "
<<
m_model
->
frames
.
at
((
pinocchio
::
Model
::
Index
)
jointId
).
name
<<
" is "
<<
tmp
<<
std
::
endl
;
...
...
@@ -795,7 +799,7 @@ computeGenericEndeffJacobian(const bool isFrame, const bool isLocal, const int j
pinocchio
::
getJointJacobian
(
*
m_model
,
*
m_data
,(
pinocchio
::
Model
::
Index
)
jointId
,
pinocchio
::
LOCAL
,
tmp
);
sotDEBUG
(
25
)
<<
"EndEffJacobian for "
<<
m_model
->
getJointN
ame
(
(
pinocchio
::
Model
::
Index
)
jointId
)
<<
m_model
->
n
ame
s
[
(
pinocchio
::
Model
::
Index
)
jointId
]
<<
" is "
<<
tmp
<<
std
::
endl
;
}
res
=
tmp
;
...
...
@@ -818,15 +822,17 @@ computeGenericPosition(const bool isFrame, const int jointId, MatrixHomogeneous&
assert
(
m_data
);
std
::
string
temp
;
forwardKinematicsSINTERN
(
time
);
if
(
isFrame
){
pinocchio
::
framesForwardKinematics
(
*
m_model
,
*
m_data
);
res
.
matrix
()
=
m_data
->
oMf
[
jointId
].
toHomogeneousMatrix
();
temp
=
m_model
->
frames
.
at
((
pinocchio
::
Model
::
Index
)
jointId
).
name
;
}
else
{
res
.
matrix
()
=
m_data
->
oMi
[
jointId
].
toHomogeneousMatrix
();
temp
=
m_model
->
getJointName
((
pinocchio
::
Model
::
Index
)
jointId
);
}
if
(
isFrame
)
{
pinocchio
::
framesForwardKinematics
(
*
m_model
,
*
m_data
);
res
.
matrix
()
=
m_data
->
oMf
[
jointId
].
toHomogeneousMatrix
();
temp
=
m_model
->
frames
.
at
((
pinocchio
::
Model
::
Index
)
jointId
).
name
;
}
else
{
res
.
matrix
()
=
m_data
->
oMi
[
jointId
].
toHomogeneousMatrix
();
temp
=
m_model
->
names
[(
pinocchio
::
Model
::
Index
)
jointId
];
}
sotDEBUG
(
25
)
<<
"For "
<<
temp
<<
" with id: "
<<
jointId
<<
" position is "
<<
res
<<
std
::
endl
;
sotDEBUGOUT
(
25
);
return
res
;
...
...
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