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
pinocchio
Commits
8207130f
Verified
Commit
8207130f
authored
Dec 16, 2019
by
Justin Carpentier
Browse files
joint/revolute: make angular rate protected
parent
dbd090f8
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/multibody/joint/joint-revolute-unbounded.hpp
View file @
8207130f
...
...
@@ -120,7 +120,7 @@ namespace pinocchio
{
calc
(
data
,
qs
.
derived
());
data
.
v
.
w
=
(
Scalar
)
vs
[
idx_v
()];
data
.
v
.
angularRate
()
=
static_cast
<
Scalar
>
(
vs
[
idx_v
()]
)
;
}
template
<
typename
Matrix6Like
>
...
...
src/multibody/joint/joint-revolute.hpp
View file @
8207130f
...
...
@@ -210,22 +210,22 @@ namespace pinocchio
MotionRevoluteTpl
()
{}
MotionRevoluteTpl
(
const
Scalar
&
w
)
:
w
(
w
)
{}
MotionRevoluteTpl
(
const
Scalar
&
w
)
:
m_
w
(
w
)
{}
template
<
typename
Vector1Like
>
MotionRevoluteTpl
(
const
Eigen
::
MatrixBase
<
Vector1Like
>
&
v
)
:
w
(
v
[
0
])
:
m_
w
(
v
[
0
])
{
using
namespace
Eigen
;
EIGEN_STATIC_ASSERT_SIZE_1x1
(
Vector1Like
);
}
inline
PlainReturnType
plain
()
const
{
return
Axis
()
*
w
;
}
inline
PlainReturnType
plain
()
const
{
return
Axis
()
*
m_
w
;
}
template
<
typename
OtherScalar
>
MotionRevoluteTpl
__mult__
(
const
OtherScalar
&
alpha
)
const
{
return
MotionRevoluteTpl
(
alpha
*
w
);
return
MotionRevoluteTpl
(
alpha
*
m_
w
);
}
template
<
typename
MotionDerived
>
...
...
@@ -233,20 +233,20 @@ namespace pinocchio
{
m
.
linear
().
setZero
();
for
(
Eigen
::
DenseIndex
k
=
0
;
k
<
3
;
++
k
)
m
.
angular
()[
k
]
=
k
==
axis
?
w
:
(
Scalar
)
0
;
m
.
angular
()[
k
]
=
k
==
axis
?
m_
w
:
(
Scalar
)
0
;
}
template
<
typename
MotionDerived
>
inline
void
addTo
(
MotionDense
<
MotionDerived
>
&
v
)
const
{
typedef
typename
MotionDense
<
MotionDerived
>::
Scalar
OtherScalar
;
v
.
angular
()[
axis
]
+=
(
OtherScalar
)
w
;
v
.
angular
()[
axis
]
+=
(
OtherScalar
)
m_
w
;
}
template
<
typename
S2
,
int
O2
,
typename
D2
>
inline
void
se3Action_impl
(
const
SE3Tpl
<
S2
,
O2
>
&
m
,
MotionDense
<
D2
>
&
v
)
const
{
v
.
angular
().
noalias
()
=
m
.
rotation
().
col
(
axis
)
*
w
;
v
.
angular
().
noalias
()
=
m
.
rotation
().
col
(
axis
)
*
m_
w
;
v
.
linear
().
noalias
()
=
m
.
translation
().
cross
(
v
.
angular
());
}
...
...
@@ -263,11 +263,11 @@ namespace pinocchio
MotionDense
<
D2
>
&
v
)
const
{
// Linear
CartesianAxis3
::
alphaCross
(
w
,
m
.
translation
(),
v
.
angular
());
CartesianAxis3
::
alphaCross
(
m_
w
,
m
.
translation
(),
v
.
angular
());
v
.
linear
().
noalias
()
=
m
.
rotation
().
transpose
()
*
v
.
angular
();
// Angular
v
.
angular
().
noalias
()
=
m
.
rotation
().
transpose
().
col
(
axis
)
*
w
;
v
.
angular
().
noalias
()
=
m
.
rotation
().
transpose
().
col
(
axis
)
*
m_
w
;
}
template
<
typename
S2
,
int
O2
>
...
...
@@ -283,10 +283,10 @@ namespace pinocchio
void
motionAction
(
const
MotionDense
<
M1
>
&
v
,
MotionDense
<
M2
>
&
mout
)
const
{
// Linear
CartesianAxis3
::
alphaCross
(
-
w
,
v
.
linear
(),
mout
.
linear
());
CartesianAxis3
::
alphaCross
(
-
m_
w
,
v
.
linear
(),
mout
.
linear
());
// Angular
CartesianAxis3
::
alphaCross
(
-
w
,
v
.
angular
(),
mout
.
angular
());
CartesianAxis3
::
alphaCross
(
-
m_
w
,
v
.
angular
(),
mout
.
angular
());
}
template
<
typename
M1
>
...
...
@@ -297,8 +297,13 @@ namespace pinocchio
return
res
;
}
Scalar
&
angularRate
()
{
return
m_w
;
}
const
Scalar
&
angularRate
()
const
{
return
m_w
;
}
// data
Scalar
w
;
protected:
Scalar
m_w
;
};
// struct MotionRevoluteTpl
template
<
typename
S1
,
int
O1
,
int
axis
,
typename
MotionDerived
>
...
...
@@ -685,7 +690,7 @@ namespace pinocchio
{
calc
(
data
,
qs
.
derived
());
data
.
v
.
w
=
(
Scalar
)
vs
[
idx_v
()];
data
.
v
.
angularRate
()
=
static_cast
<
Scalar
>
(
vs
[
idx_v
()]
)
;
}
template
<
typename
Matrix6Like
>
...
...
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