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
c0969180
Verified
Commit
c0969180
authored
Dec 17, 2019
by
Justin Carpentier
Browse files
joints/consraint: set axis as protected member
parent
0a8689e1
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/multibody/joint/joint-prismatic-unaligned.hpp
View file @
c0969180
...
...
@@ -238,14 +238,14 @@ namespace pinocchio
template
<
typename
Vector3Like
>
ConstraintPrismaticUnalignedTpl
(
const
Eigen
::
MatrixBase
<
Vector3Like
>
&
axis
)
:
axis
(
axis
)
:
m_
axis
(
axis
)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
(
Vector3Like
,
3
);
}
template
<
typename
Vector1Like
>
JointMotion
__mult__
(
const
Eigen
::
MatrixBase
<
Vector1Like
>
&
v
)
const
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
(
Vector1Like
,
1
);
return
JointMotion
(
axis
,
v
[
0
]);
return
JointMotion
(
m_
axis
,
v
[
0
]);
}
template
<
typename
S1
,
int
O1
>
...
...
@@ -254,7 +254,7 @@ namespace pinocchio
{
typename
SE3GroupAction
<
ConstraintPrismaticUnalignedTpl
>::
ReturnType
res
;
MotionRef
<
DenseBase
>
v
(
res
);
v
.
linear
().
noalias
()
=
m
.
rotation
()
*
axis
;
v
.
linear
().
noalias
()
=
m
.
rotation
()
*
m_
axis
;
v
.
angular
().
setZero
();
return
res
;
}
...
...
@@ -265,7 +265,7 @@ namespace pinocchio
{
typename
SE3GroupAction
<
ConstraintPrismaticUnalignedTpl
>::
ReturnType
res
;
MotionRef
<
DenseBase
>
v
(
res
);
v
.
linear
().
noalias
()
=
m
.
rotation
().
transpose
()
*
axis
;
v
.
linear
().
noalias
()
=
m
.
rotation
().
transpose
()
*
m_
axis
;
v
.
angular
().
setZero
();
return
res
;
}
...
...
@@ -283,7 +283,7 @@ namespace pinocchio
{
typedef
typename
ConstraintForceOp
<
ConstraintPrismaticUnalignedTpl
,
ForceDerived
>::
ReturnType
ReturnType
;
ReturnType
res
;
res
[
0
]
=
ref
.
axis
.
dot
(
f
.
linear
());
res
[
0
]
=
ref
.
axis
()
.
dot
(
f
.
linear
());
return
res
;
}
...
...
@@ -294,12 +294,12 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT
(
ForceSet
::
RowsAtCompileTime
==
6
,
THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE
)
/* Return ax.T * F[1:3,:] */
return
ref
.
axis
.
transpose
()
*
F
.
template
middleRows
<
3
>(
LINEAR
);
return
ref
.
axis
()
.
transpose
()
*
F
.
template
middleRows
<
3
>(
LINEAR
);
}
};
TransposeConst
transpose
()
const
{
return
TransposeConst
(
*
this
);
}
TransposeConst
transpose
()
const
{
return
TransposeConst
(
*
this
);
}
/* CRBA joint operators
* - ForceSet::Block = ForceSet
...
...
@@ -310,7 +310,8 @@ namespace pinocchio
DenseBase
matrix_impl
()
const
{
DenseBase
S
;
S
<<
axis
,
Vector3
::
Zero
();
S
.
template
segment
<
3
>(
LINEAR
)
=
m_axis
;
S
.
template
segment
<
3
>(
ANGULAR
).
setZero
();
return
S
;
}
...
...
@@ -318,13 +319,18 @@ namespace pinocchio
DenseBase
motionAction
(
const
MotionDense
<
MotionDerived
>
&
v
)
const
{
DenseBase
res
;
res
<<
v
.
angular
().
cross
(
axis
),
Vector3
::
Zero
();
res
.
template
segment
<
3
>(
LINEAR
).
noalias
()
=
v
.
angular
().
cross
(
m_axis
);
res
.
template
segment
<
3
>(
ANGULAR
).
setZero
();
return
res
;
}
const
Vector3
&
axis
()
const
{
return
m_axis
;
}
Vector3
&
axis
()
{
return
m_axis
;
}
// data
Vector3
axis
;
protected:
Vector3
m_axis
;
};
// struct ConstraintPrismaticUnalignedTpl
...
...
@@ -352,8 +358,8 @@ namespace pinocchio
const
S1
&
m
=
Y
.
mass
();
const
typename
Inertia
::
Vector3
&
c
=
Y
.
lever
();
res
.
template
head
<
3
>(
).
noalias
()
=
m
*
cpu
.
axis
;
res
.
template
tail
<
3
>(
).
noalias
()
=
c
.
cross
(
res
.
template
head
<
3
>(
));
res
.
template
segment
<
3
>(
Constraint
::
LINEAR
).
noalias
()
=
m
*
cpu
.
axis
()
;
res
.
template
segment
<
3
>(
Constraint
::
ANGULAR
).
noalias
()
=
c
.
cross
(
res
.
template
segment
<
3
>(
Constraint
::
LINEAR
));
return
res
;
}
...
...
@@ -383,7 +389,7 @@ namespace pinocchio
const
Constraint
&
cru
)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE
(
M6Like
,
6
,
6
);
return
Y
.
derived
().
template
middleCols
<
3
>(
Constraint
::
LINEAR
)
*
cru
.
axis
;
return
Y
.
derived
().
template
middleCols
<
3
>(
Constraint
::
LINEAR
)
*
cru
.
axis
()
;
}
};
}
// namespace impl
...
...
src/multibody/joint/joint-revolute-unaligned.hpp
View file @
c0969180
...
...
@@ -249,14 +249,14 @@ namespace pinocchio
template
<
typename
Vector3Like
>
ConstraintRevoluteUnalignedTpl
(
const
Eigen
::
MatrixBase
<
Vector3Like
>
&
axis
)
:
axis
(
axis
)
:
m_
axis
(
axis
)
{}
template
<
typename
Vector1Like
>
JointMotion
__mult__
(
const
Eigen
::
MatrixBase
<
Vector1Like
>
&
v
)
const
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
(
Vector1Like
,
1
);
return
JointMotion
(
axis
,
v
[
0
]);
return
JointMotion
(
m_
axis
,
v
[
0
]);
}
template
<
typename
S1
,
int
O1
>
...
...
@@ -267,7 +267,7 @@ namespace pinocchio
/* X*S = [ R pxR ; 0 R ] [ 0 ; a ] = [ px(Ra) ; Ra ] */
ReturnType
res
;
res
.
template
segment
<
3
>(
ANGULAR
).
noalias
()
=
m
.
rotation
()
*
axis
;
res
.
template
segment
<
3
>(
ANGULAR
).
noalias
()
=
m
.
rotation
()
*
m_
axis
;
res
.
template
segment
<
3
>(
LINEAR
).
noalias
()
=
m
.
translation
().
cross
(
res
.
template
segment
<
3
>(
ANGULAR
));
return
res
;
}
...
...
@@ -279,8 +279,8 @@ namespace pinocchio
typedef
typename
SE3GroupAction
<
ConstraintRevoluteUnalignedTpl
>::
ReturnType
ReturnType
;
ReturnType
res
;
res
.
template
segment
<
3
>(
ANGULAR
).
noalias
()
=
m
.
rotation
().
transpose
()
*
axis
;
res
.
template
segment
<
3
>(
LINEAR
).
noalias
()
=
-
m
.
rotation
().
transpose
()
*
m
.
translation
().
cross
(
axis
);
res
.
template
segment
<
3
>(
ANGULAR
).
noalias
()
=
m
.
rotation
().
transpose
()
*
m_
axis
;
res
.
template
segment
<
3
>(
LINEAR
).
noalias
()
=
-
m
.
rotation
().
transpose
()
*
m
.
translation
().
cross
(
m_
axis
);
return
res
;
}
...
...
@@ -297,7 +297,7 @@ namespace pinocchio
{
typedef
typename
ConstraintForceOp
<
ConstraintRevoluteUnalignedTpl
,
ForceDerived
>::
ReturnType
ReturnType
;
ReturnType
res
;
res
[
0
]
=
ref
.
axis
.
dot
(
f
.
angular
());
res
[
0
]
=
ref
.
axis
()
.
dot
(
f
.
angular
());
return
res
;
}
...
...
@@ -308,7 +308,7 @@ namespace pinocchio
{
EIGEN_STATIC_ASSERT
(
ForceSet
::
RowsAtCompileTime
==
6
,
THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE
)
/* Return ax.T * F[3:end,:] */
return
ref
.
axis
.
transpose
()
*
F
.
template
middleRows
<
3
>(
ANGULAR
);
return
ref
.
axis
()
.
transpose
()
*
F
.
template
middleRows
<
3
>(
ANGULAR
);
}
};
...
...
@@ -324,7 +324,8 @@ namespace pinocchio
DenseBase
matrix_impl
()
const
{
DenseBase
S
;
S
<<
Vector3
::
Zero
(),
axis
;
S
.
template
segment
<
3
>(
LINEAR
).
setZero
();
S
.
template
segment
<
3
>(
ANGULAR
)
=
m_axis
;
return
S
;
}
...
...
@@ -336,13 +337,18 @@ namespace pinocchio
const
typename
MotionDerived
::
ConstAngularType
w
=
m
.
angular
();
DenseBase
res
;
res
<<
v
.
cross
(
axis
),
w
.
cross
(
axis
);
res
.
template
segment
<
3
>(
LINEAR
).
noalias
()
=
v
.
cross
(
m_axis
);
res
.
template
segment
<
3
>(
ANGULAR
).
noalias
()
=
w
.
cross
(
m_axis
);
return
res
;
}
// data
Vector3
axis
;
const
Vector3
&
axis
()
const
{
return
m_axis
;
}
Vector3
&
axis
()
{
return
m_axis
;
}
protected:
Vector3
m_axis
;
};
// struct ConstraintRevoluteUnalignedTpl
...
...
@@ -371,8 +377,8 @@ namespace pinocchio
const
typename
Inertia
::
Vector3
&
c
=
Y
.
lever
();
const
typename
Inertia
::
Symmetric3
&
I
=
Y
.
inertia
();
res
.
template
segment
<
3
>(
Inertia
::
LINEAR
)
=
-
m
*
c
.
cross
(
cru
.
axis
);
res
.
template
segment
<
3
>(
Inertia
::
ANGULAR
).
noalias
()
=
I
*
cru
.
axis
;
res
.
template
segment
<
3
>(
Inertia
::
LINEAR
)
=
-
m
*
c
.
cross
(
cru
.
axis
()
);
res
.
template
segment
<
3
>(
Inertia
::
ANGULAR
).
noalias
()
=
I
*
cru
.
axis
()
;
res
.
template
segment
<
3
>(
Inertia
::
ANGULAR
)
+=
c
.
cross
(
res
.
template
segment
<
3
>(
Inertia
::
LINEAR
));
return
res
;
...
...
@@ -404,7 +410,7 @@ namespace pinocchio
const
Constraint
&
cru
)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE
(
M6Like
,
6
,
6
);
return
Y
.
derived
().
template
middleCols
<
3
>(
Constraint
::
ANGULAR
)
*
cru
.
axis
;
return
Y
.
derived
().
template
middleCols
<
3
>(
Constraint
::
ANGULAR
)
*
cru
.
axis
()
;
}
};
}
// namespace impl
...
...
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