Skip to content
GitLab
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
pinocchio
Commits
36cb1069
Commit
36cb1069
authored
Jan 15, 2018
by
jcarpent
Browse files
[Model] Use o{f,h} instead of local {f,h}
parent
92519469
Changes
3
Hide whitespace changes
Inline
Side-by-side
src/algorithm/rnea-derivatives.hxx
View file @
36cb1069
...
...
@@ -54,7 +54,7 @@ namespace se3
data
.
oMi
[
i
]
=
data
.
liMi
[
i
];
data
.
oYcrb
[
i
]
=
data
.
oMi
[
i
].
act
(
model
.
inertias
[
i
]);
data
.
f
[
i
].
setZero
();
data
.
o
f
[
i
].
setZero
();
typedef
typename
SizeDepType
<
JointModel
::
NV
>::
template
ColsReturn
<
Data
::
Matrix6x
>
::
Type
ColsBlock
;
ColsBlock
J_cols
=
jmodel
.
jointCols
(
data
.
J
);
...
...
@@ -97,14 +97,14 @@ namespace se3
gravity_partial_dq
.
block
(
jmodel
.
idx_v
(),
jmodel
.
idx_v
(),
jmodel
.
nv
(),
data
.
nvSubtree
[
i
]).
noalias
()
=
J_cols
.
transpose
()
*
data
.
dFdq
.
middleCols
(
jmodel
.
idx_v
(),
data
.
nvSubtree
[
i
]);
data
.
f
[
i
]
=
data
.
oYcrb
[
i
]
*
oa
;
motionSet
::
act
<
ADDTO
>
(
J_cols
,
data
.
f
[
i
],
dFdq_cols
);
data
.
o
f
[
i
]
=
data
.
oYcrb
[
i
]
*
oa
;
motionSet
::
act
<
ADDTO
>
(
J_cols
,
data
.
o
f
[
i
],
dFdq_cols
);
lhsInertiaMult
(
data
.
oYcrb
[
i
],
J_cols
.
transpose
(),
M6tmpR
.
topRows
(
jmodel
.
nv
()));
for
(
int
j
=
data
.
parents_fromRow
[(
Model
::
Index
)
jmodel
.
idx_v
()];
j
>=
0
;
j
=
data
.
parents_fromRow
[(
Model
::
Index
)
j
])
gravity_partial_dq
.
middleRows
(
jmodel
.
idx_v
(),
jmodel
.
nv
()).
col
(
j
).
noalias
()
=
M6tmpR
.
topRows
(
jmodel
.
nv
())
*
data
.
dAdq
.
col
(
j
);
jmodel
.
jointVelocitySelector
(
data
.
g
).
noalias
()
=
J_cols
.
transpose
()
*
data
.
f
[
i
].
toVector
();
jmodel
.
jointVelocitySelector
(
data
.
g
).
noalias
()
=
J_cols
.
transpose
()
*
data
.
o
f
[
i
].
toVector
();
if
(
parent
>
0
)
{
data
.
oYcrb
[
parent
]
+=
data
.
oYcrb
[
i
];
...
...
@@ -195,8 +195,8 @@ namespace se3
ov
=
data
.
oMi
[
i
].
act
(
data
.
v
[
i
]);
oa
=
data
.
oMi
[
i
].
act
(
data
.
a
[
i
])
+
data
.
oa
[
0
];
// add gravity contribution
data
.
h
[
i
]
=
data
.
oYcrb
[
i
]
*
ov
;
data
.
f
[
i
]
=
data
.
oYcrb
[
i
]
*
oa
+
ov
.
cross
(
data
.
h
[
i
]);
data
.
o
h
[
i
]
=
data
.
oYcrb
[
i
]
*
ov
;
data
.
o
f
[
i
]
=
data
.
oYcrb
[
i
]
*
oa
+
ov
.
cross
(
data
.
o
h
[
i
]);
typedef
typename
SizeDepType
<
JointModel
::
NV
>::
template
ColsReturn
<
Data
::
Matrix6x
>
::
Type
ColsBlock
;
ColsBlock
J_cols
=
jmodel
.
jointCols
(
data
.
J
);
...
...
@@ -221,7 +221,7 @@ namespace se3
// computes variation of inertias
data
.
doYcrb
[
i
]
=
data
.
oYcrb
[
i
].
variation
(
ov
);
addForceCrossMatrix
(
data
.
h
[
i
],
data
.
doYcrb
[
i
]);
addForceCrossMatrix
(
data
.
o
h
[
i
],
data
.
doYcrb
[
i
]);
}
template
<
typename
ForceDerived
,
typename
M6
>
...
...
@@ -271,7 +271,7 @@ namespace se3
ColsBlock
dFda_cols
=
jmodel
.
jointCols
(
data
.
dFda
);
// tau
jmodel
.
jointVelocitySelector
(
data
.
tau
).
noalias
()
=
J_cols
.
transpose
()
*
data
.
f
[
i
].
toVector
();
jmodel
.
jointVelocitySelector
(
data
.
tau
).
noalias
()
=
J_cols
.
transpose
()
*
data
.
o
f
[
i
].
toVector
();
// dtau/da similar to data.M
motionSet
::
inertiaAction
(
data
.
oYcrb
[
i
],
J_cols
,
dFda_cols
);
...
...
@@ -293,7 +293,7 @@ namespace se3
rnea_partial_dq
.
block
(
jmodel
.
idx_v
(),
jmodel
.
idx_v
(),
jmodel
.
nv
(),
data
.
nvSubtree
[
i
]).
noalias
()
=
J_cols
.
transpose
()
*
data
.
dFdq
.
middleCols
(
jmodel
.
idx_v
(),
data
.
nvSubtree
[
i
]);
motionSet
::
act
<
ADDTO
>
(
J_cols
,
data
.
f
[
i
],
dFdq_cols
);
motionSet
::
act
<
ADDTO
>
(
J_cols
,
data
.
o
f
[
i
],
dFdq_cols
);
if
(
parent
>
0
)
{
...
...
@@ -314,7 +314,7 @@ namespace se3
{
data
.
oYcrb
[
parent
]
+=
data
.
oYcrb
[
i
];
data
.
doYcrb
[
parent
]
+=
data
.
doYcrb
[
i
];
data
.
f
[
parent
]
+=
data
.
f
[
i
];
data
.
o
f
[
parent
]
+=
data
.
o
f
[
i
];
}
}
...
...
src/multibody/model.hpp
View file @
36cb1069
...
...
@@ -379,13 +379,20 @@ namespace se3
/// \brief Vector of joint velocities expressed at the origin.
container
::
aligned_vector
<
Motion
>
ov
;
/// \brief Vector of body forces. For each body, the force represents the sum of
/// all external forces acting on the body
and expressed in the local frame of the joint.
.
/// \brief Vector of body forces
expressed in the local frame of the joint
. For each body, the force represents the sum of
/// all external forces acting on the body.
container
::
aligned_vector
<
Force
>
f
;
/// \brief Vector of spatial momenta.
/// \brief Vector of body forces expressed in the world frame. For each body, the force represents the sum of
/// all external forces acting on the body.
container
::
aligned_vector
<
Force
>
of
;
/// \brief Vector of spatial momenta expressed in the local frame of the joint.
container
::
aligned_vector
<
Force
>
h
;
/// \brief Vector of spatial momenta expressed in the world frame.
container
::
aligned_vector
<
Force
>
oh
;
/// \brief Vector of absolute joint placements (wrt the world).
container
::
aligned_vector
<
SE3
>
oMi
;
...
...
src/multibody/model.hxx
View file @
36cb1069
...
...
@@ -240,7 +240,9 @@ namespace se3
,
v
((
std
::
size_t
)
model
.
njoints
)
,
ov
((
std
::
size_t
)
model
.
njoints
)
,
f
((
std
::
size_t
)
model
.
njoints
)
,
of
((
std
::
size_t
)
model
.
njoints
)
,
h
((
std
::
size_t
)
model
.
njoints
)
,
oh
((
std
::
size_t
)
model
.
njoints
)
,
oMi
((
std
::
size_t
)
model
.
njoints
)
,
liMi
((
std
::
size_t
)
model
.
njoints
)
,
tau
(
model
.
nv
)
...
...
Write
Preview
Supports
Markdown
0%
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!
Cancel
Please
register
or
sign in
to comment