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
Gabriele Buondonno
pinocchio
Commits
31562102
Commit
31562102
authored
Sep 27, 2017
by
jcarpent
Browse files
[Joints] Add variation operation for Constraints struct
parent
bf00b109
Changes
11
Hide whitespace changes
Inline
Side-by-side
src/multibody/constraint.hpp
View file @
31562102
...
...
@@ -189,6 +189,13 @@ namespace se3
{
return
(
m
.
inverse
().
toActionMatrix
()
*
S
).
eval
();
}
DenseBase
variation
(
const
Motion
&
v
)
const
{
DenseBase
res
(
v
.
toActionMatrix
()
*
S
);
return
res
;
}
void
disp_impl
(
std
::
ostream
&
os
)
const
{
os
<<
"S =
\n
"
<<
S
<<
std
::
endl
;}
...
...
src/multibody/joint/joint-free-flyer.hpp
View file @
31562102
...
...
@@ -84,6 +84,8 @@ namespace se3
TransposeConst
transpose
()
const
{
return
TransposeConst
();
}
operator
ConstraintXd
()
const
{
return
ConstraintXd
(
SE3
::
Matrix6
::
Identity
());
}
DenseBase
variation
(
const
Motion
&
m
)
const
{
return
m
.
toActionMatrix
();
}
};
// struct ConstraintIdentity
template
<
typename
D
>
...
...
src/multibody/joint/joint-planar.hpp
View file @
31562102
...
...
@@ -189,6 +189,20 @@ namespace se3
return
X_subspace
;
}
DenseBase
variation
(
const
Motion
&
m
)
const
{
const
Motion
::
ConstLinear_t
v
=
m
.
linear
();
const
Motion
::
ConstAngular_t
w
=
m
.
angular
();
DenseBase
res
(
DenseBase
::
Zero
());
res
(
0
,
1
)
=
-
w
[
2
];
res
(
0
,
2
)
=
v
[
1
];
res
(
1
,
0
)
=
w
[
2
];
res
(
1
,
2
)
=
-
v
[
0
];
res
(
2
,
0
)
=
-
w
[
1
];
res
(
2
,
1
)
=
w
[
0
];
res
(
3
,
2
)
=
w
[
1
];
res
(
4
,
2
)
=
-
w
[
0
];
return
res
;
}
};
// struct ConstraintPlanar
template
<
typename
D
>
...
...
src/multibody/joint/joint-prismatic-unaligned.hpp
View file @
31562102
...
...
@@ -191,6 +191,14 @@ namespace se3
return
ConstraintXd
(
S
);
}
DenseBase
variation
(
const
Motion
&
m
)
const
{
DenseBase
res
;
res
<<
m
.
angular
().
cross
(
axis
),
Vector3
::
Zero
();
return
res
;
}
};
// struct ConstraintPrismaticUnaligned
...
...
src/multibody/joint/joint-prismatic.hpp
View file @
31562102
...
...
@@ -196,6 +196,14 @@ namespace se3
S
<<
prismatic
::
CartesianVector3
<
axis
>
(
1
).
vector
(),
Eigen
::
Vector3d
::
Zero
();
return
ConstraintXd
(
S
);
}
DenseBase
variation
(
const
Motion
&
m
)
const
{
DenseBase
res
;
res
<<
m
.
angular
().
cross
(
prismatic
::
CartesianVector3
<
axis
>
(
1
).
vector
()),
Vector3
::
Zero
();
return
res
;
}
};
// struct ConstraintPrismatic
...
...
src/multibody/joint/joint-revolute-unaligned.hpp
View file @
31562102
...
...
@@ -192,6 +192,17 @@ namespace se3
return
ConstraintXd
(
S
);
}
DenseBase
variation
(
const
Motion
&
m
)
const
{
const
Motion
::
ConstLinear_t
v
=
m
.
linear
();
const
Motion
::
ConstAngular_t
w
=
m
.
angular
();
DenseBase
res
;
res
<<
v
.
cross
(
axis
),
w
.
cross
(
axis
);
return
res
;
}
};
// struct ConstraintRevoluteUnaligned
...
...
src/multibody/joint/joint-revolute.hpp
View file @
31562102
...
...
@@ -198,6 +198,18 @@ namespace se3
S
<<
Eigen
::
Vector3d
::
Zero
(),
revolute
::
CartesianVector3
<
axis
>
(
1
).
vector
();
return
ConstraintXd
(
S
);
}
DenseBase
variation
(
const
Motion
&
m
)
const
{
const
typename
Motion
::
ConstLinear_t
v
=
m
.
linear
();
const
typename
Motion
::
ConstAngular_t
w
=
m
.
angular
();
const
Vector3
a
(
revolute
::
CartesianVector3
<
axis
>
(
1
).
vector
());
DenseBase
res
;
res
<<
v
.
cross
(
a
),
w
.
cross
(
a
);
return
res
;
}
};
// struct ConstraintRevolute
template
<
int
axis
>
...
...
src/multibody/joint/joint-spherical-ZYX.hpp
View file @
31562102
...
...
@@ -97,6 +97,7 @@ namespace se3
typedef
Eigen
::
Matrix
<
_Scalar
,
3
,
3
,
_Options
>
Matrix3
;
typedef
Eigen
::
Matrix
<
_Scalar
,
3
,
1
,
_Options
>
Vector3
;
typedef
Eigen
::
Matrix
<
_Scalar
,
6
,
3
,
_Options
>
ConstraintDense
;
typedef
Eigen
::
Matrix
<
_Scalar
,
6
,
3
,
_Options
>
DenseBase
;
Matrix3
S_minimal
;
...
...
@@ -187,6 +188,18 @@ namespace se3
return
result
;
}
DenseBase
variation
(
const
Motion
&
m
)
const
{
const
typename
Motion
::
ConstLinear_t
v
=
m
.
linear
();
const
typename
Motion
::
ConstAngular_t
w
=
m
.
angular
();
DenseBase
res
;
res
.
template
middleRows
<
3
>(
Motion
::
LINEAR
)
=
cross
(
v
,
S_minimal
);
res
.
template
middleRows
<
3
>(
Motion
::
ANGULAR
)
=
cross
(
w
,
S_minimal
);
return
res
;
}
};
// struct ConstraintRotationalSubspace
...
...
src/multibody/joint/joint-spherical.hpp
View file @
31562102
...
...
@@ -156,11 +156,23 @@ namespace se3
Eigen
::
Matrix
<
double
,
6
,
3
>
se3Action
(
const
SE3
&
m
)
const
{
Eigen
::
Matrix
<
double
,
6
,
3
>
X_subspace
;
X_subspace
.
block
<
3
,
3
>
(
Motion
::
LINEAR
,
0
)
=
skew
(
m
.
translation
()
)
*
m
.
rotation
();
X_subspace
.
block
<
3
,
3
>
(
Motion
::
LINEAR
,
0
)
=
cross
(
m
.
translation
()
,
m
.
rotation
()
)
;
X_subspace
.
block
<
3
,
3
>
(
Motion
::
ANGULAR
,
0
)
=
m
.
rotation
();
return
X_subspace
;
}
DenseBase
variation
(
const
Motion
&
m
)
const
{
const
Motion
::
ConstLinear_t
v
=
m
.
linear
();
const
Motion
::
ConstAngular_t
w
=
m
.
angular
();
DenseBase
res
;
res
.
middleRows
<
3
>
(
LINEAR
)
=
skew
(
v
);
res
.
middleRows
<
3
>
(
ANGULAR
)
=
skew
(
w
);
return
res
;
}
};
// struct ConstraintRotationalSubspace
...
...
src/multibody/joint/joint-translation.hpp
View file @
31562102
...
...
@@ -172,6 +172,17 @@ namespace se3
return
M
;
}
DenseBase
variation
(
const
Motion
&
m
)
const
{
const
Motion
::
ConstAngular_t
w
=
m
.
angular
();
DenseBase
res
;
res
.
middleRows
<
3
>
(
LINEAR
)
=
skew
(
w
);
res
.
middleRows
<
3
>
(
ANGULAR
).
setZero
();
return
res
;
}
};
// struct ConstraintTranslationSubspace
...
...
unittest/joint.cpp
View file @
31562102
...
...
@@ -23,8 +23,8 @@
using
namespace
se3
;
template
<
typename
T
>
void
test_joint_methods
(
T
&
jmodel
,
typename
T
::
JointDataDerived
&
jdata
)
template
<
typename
JointModel
>
void
test_joint_methods
(
JointModel
&
jmodel
,
typename
JointModel
::
JointDataDerived
&
jdata
)
{
std
::
cout
<<
"Testing Joint over "
<<
jmodel
.
shortname
()
<<
std
::
endl
;
Eigen
::
VectorXd
q1
(
Eigen
::
VectorXd
::
Random
(
jmodel
.
nq
()));
...
...
@@ -75,6 +75,18 @@ void test_joint_methods (T & jmodel, typename T::JointDataDerived & jdata)
BOOST_CHECK_MESSAGE
((
jda
.
U
()).
isApprox
(
jdata
.
U
),
std
::
string
(
error_prefix
+
" - Joint U inertia matrix decomposition "
));
BOOST_CHECK_MESSAGE
((
jda
.
Dinv
()).
isApprox
(
jdata
.
Dinv
),
std
::
string
(
error_prefix
+
" - Joint DInv inertia matrix decomposition "
));
BOOST_CHECK_MESSAGE
((
jda
.
UDinv
()).
isApprox
(
jdata
.
UDinv
),
std
::
string
(
error_prefix
+
" - Joint UDInv inertia matrix decomposition "
));
// Test vxS
typedef
typename
JointModel
::
Constraint_t
Constraint_t
;
typedef
typename
Constraint_t
::
DenseBase
ConstraintDense
;
Motion
v
(
Motion
::
Random
());
ConstraintDense
vxS
(
jdata
.
S
.
variation
(
v
));
ConstraintDense
vxS_ref
=
v
.
toActionMatrix
()
*
ConstraintXd
(
jdata
.
S
).
matrix
();
BOOST_CHECK_MESSAGE
(
vxS
.
isApprox
(
vxS_ref
),
std
::
string
(
error_prefix
+
"- Joint vxS operation "
));
}
struct
TestJoint
{
...
...
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