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
2e414ab5
Commit
2e414ab5
authored
Sep 27, 2017
by
jcarpent
Browse files
[Spatial] Add scalar multiplication of a motion vector
parent
5170c96b
Changes
2
Hide whitespace changes
Inline
Side-by-side
src/spatial/motion.hpp
View file @
2e414ab5
//
// Copyright (c) 2015-201
6
CNRS
// Copyright (c) 2015-201
7
CNRS
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//
// This file is part of Pinocchio
...
...
@@ -65,6 +65,7 @@ namespace se3
Derived_t
operator
-
(
const
Derived_t
&
v2
)
const
{
return
derived
().
__minus__
(
v2
);
}
Derived_t
&
operator
+=
(
const
Derived_t
&
v2
)
{
return
derived
().
__pequ__
(
v2
);
}
Derived_t
&
operator
-=
(
const
Derived_t
&
v2
)
{
return
derived
().
__mequ__
(
v2
);
}
Derived_t
operator
*
(
const
Scalar
alpha
)
const
{
return
derived
().
__mult__
(
alpha
);
}
bool
isApprox
(
const
Derived_t
&
other
,
const
Scalar
&
prec
=
Eigen
::
NumTraits
<
Scalar
>::
dummy_precision
())
const
{
return
derived
().
isApprox_impl
(
other
,
prec
);}
...
...
@@ -206,6 +207,7 @@ namespace se3
MotionTpl
__minus__
(
const
MotionTpl
&
v2
)
const
{
return
MotionTpl
(
data
-
v2
.
data
);
}
MotionTpl
&
__pequ__
(
const
MotionTpl
&
v2
)
{
data
+=
v2
.
data
;
return
*
this
;
}
MotionTpl
&
__mequ__
(
const
MotionTpl
&
v2
)
{
data
-=
v2
.
data
;
return
*
this
;
}
MotionTpl
__mult__
(
const
Scalar
alpha
)
const
{
return
MotionTpl
(
alpha
*
data
);
}
Scalar
dot
(
const
Force
&
f
)
const
{
return
data
.
dot
(
f
.
toVector
());
}
...
...
@@ -269,6 +271,10 @@ namespace se3
MotionTpl
<
S
,
O
>
operator
^
(
const
MotionTpl
<
S
,
O
>
&
m1
,
const
MotionTpl
<
S
,
O
>
&
m2
)
{
return
m1
.
cross
(
m2
);
}
template
<
typename
S
,
int
O
>
ForceTpl
<
S
,
O
>
operator
^
(
const
MotionTpl
<
S
,
O
>
&
m
,
const
ForceTpl
<
S
,
O
>
&
f
)
{
return
m
.
cross
(
f
);
}
template
<
typename
S
,
int
O
>
MotionTpl
<
S
,
O
>
operator
*
(
const
S
alpha
,
const
MotionTpl
<
S
,
O
>
&
m
)
{
return
m
*
alpha
;
}
/////////////// BiasZero ///////////////
...
...
unittest/tspatial.cpp
View file @
2e414ab5
...
...
@@ -112,6 +112,10 @@ BOOST_AUTO_TEST_CASE ( test_Motion )
// Test .=V6
bv3
=
bv2_vec
;
BOOST_CHECK
(
bv3
.
toVector
().
isApprox
(
bv2_vec
,
1e-12
));
// Test scalar*M6
Motion
twicebv
(
2.
*
bv
);
BOOST_CHECK
(
twicebv
.
isApprox
(
Motion
(
2.
*
bv
.
toVector
())));
// Test constructor from V6
Motion
bv4
(
bv2_vec
);
...
...
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