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
Guilhem Saurel
pinocchio
Commits
ffe8faa7
Commit
ffe8faa7
authored
Sep 19, 2016
by
Justin Carpentier
Committed by
GitHub
Sep 19, 2016
Browse files
Merge pull request #326 from fvalenza/topic/JointComposite
Topic/joint composite
parents
3b7906d9
abfd018a
Changes
23
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
ffe8faa7
...
...
@@ -156,6 +156,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint.hpp
multibody/joint/joint-basic-visitors.hpp
multibody/joint/joint-basic-visitors.hxx
multibody/joint/joint-composite.hpp
)
SET
(
${
PROJECT_NAME
}
_MULTIBODY_HEADERS
...
...
bindings/python/joint.hpp
View file @
ffe8faa7
...
...
@@ -20,7 +20,7 @@
#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
#include "pinocchio/multibody/joint/joint.hpp"
#include "pinocchio/multibody/joint/joint
-composite
.hpp"
namespace
se3
{
...
...
src/algorithm/center-of-mass.hxx
View file @
ffe8faa7
...
...
@@ -225,14 +225,37 @@ namespace se3
ColBlock
Jcols
=
jmodel
.
jointCols
(
data
.
J
);
Jcols
=
data
.
oMi
[
i
].
act
(
jdata
.
S
());
if
(
JointModel
::
NV
==
1
)
if
(
JointModel
::
NV
==
-
1
)
{
if
(
jmodel
.
nv
()
==
1
)
{
data
.
Jcom
.
col
(
jmodel
.
idx_v
())
=
data
.
mass
[
i
]
*
Jcols
.
template
topLeftCorner
<
3
,
1
>()
-
data
.
com
[
i
].
cross
(
Jcols
.
template
bottomLeftCorner
<
3
,
1
>())
;
}
else
{
jmodel
.
jointCols
(
data
.
Jcom
)
=
data
.
mass
[
i
]
*
Jcols
.
template
topRows
<
3
>()
-
skew
(
data
.
com
[
i
])
*
Jcols
.
template
bottomRows
<
3
>();
}
}
else
jmodel
.
jointCols
(
data
.
Jcom
)
=
data
.
mass
[
i
]
*
Jcols
.
template
topRows
<
3
>()
-
skew
(
data
.
com
[
i
])
*
Jcols
.
template
bottomRows
<
3
>();
{
if
(
JointModel
::
NV
==
1
)
{
data
.
Jcom
.
col
(
jmodel
.
idx_v
())
=
data
.
mass
[
i
]
*
Jcols
.
template
topLeftCorner
<
3
,
1
>()
-
data
.
com
[
i
].
cross
(
Jcols
.
template
bottomLeftCorner
<
3
,
1
>())
;
}
else
{
jmodel
.
jointCols
(
data
.
Jcom
)
=
data
.
mass
[
i
]
*
Jcols
.
template
topRows
<
3
>()
-
skew
(
data
.
com
[
i
])
*
Jcols
.
template
bottomRows
<
3
>();
}
}
if
(
computeSubtreeComs
)
data
.
com
[
i
]
/=
data
.
mass
[
i
];
...
...
src/algorithm/crba.hxx
View file @
ffe8faa7
...
...
@@ -161,7 +161,7 @@ namespace se3
const
se3
::
Model
&
model
,
se3
::
Data
&
data
)
{
typedef
typename
Data
::
Matrix6x
::
NColsBlockXpr
<
JointModel
::
NV
>::
Type
ColsBlock
;
typedef
typename
SizeDepType
<
JointModel
::
NV
>::
template
ColsReturn
<
Data
::
Matrix6x
>
::
Type
ColsBlock
;
const
Model
::
JointIndex
&
i
=
(
Model
::
JointIndex
)
jmodel
.
id
();
const
Model
::
Index
&
parent
=
model
.
parents
[
i
];
...
...
@@ -169,11 +169,33 @@ namespace se3
data
.
Ycrb
[
parent
]
+=
data
.
liMi
[
i
].
act
(
data
.
Ycrb
[
i
]);
jdata
.
U
()
=
data
.
Ycrb
[
i
]
*
jdata
.
S
();
ColsBlock
jF
=
data
.
Ag
.
middleCols
<
JointModel
::
NV
>
(
jmodel
.
idx_v
());
=
data
.
Ag
.
middleCols
<
JointModel
::
NV
>
(
jmodel
.
idx_v
());
forceSet
::
se3Action
(
data
.
oMi
[
i
],
jdata
.
U
(),
jF
);
}
static
void
algo
(
const
se3
::
JointModelBase
<
JointModelComposite
>
&
jmodel
,
se3
::
JointDataBase
<
JointDataComposite
>
&
jdata
,
const
se3
::
Model
&
model
,
se3
::
Data
&
data
)
{
typedef
SizeDepType
<
JointModel
::
NV
>::
ColsReturn
<
Data
::
Matrix6x
>::
Type
ColsBlock
;
const
Model
::
JointIndex
&
i
=
(
Model
::
JointIndex
)
jmodel
.
id
();
const
Model
::
Index
&
parent
=
model
.
parents
[
i
];
data
.
Ycrb
[
parent
]
+=
data
.
liMi
[
i
].
act
(
data
.
Ycrb
[
i
]);
jdata
.
U
()
=
data
.
Ycrb
[
i
]
*
jdata
.
S
();
ColsBlock
jF
=
data
.
Ag
.
middleCols
(
jmodel
.
idx_v
(),
jmodel
.
nv
());
forceSet
::
se3Action
(
data
.
oMi
[
i
],
jdata
.
U
(),
jF
);
}
};
// struct CcrbaBackwardStep
inline
const
Data
::
Matrix6x
&
...
...
src/algorithm/joint-configuration.hpp
View file @
ffe8faa7
...
...
@@ -146,7 +146,6 @@ namespace se3
const
Eigen
::
VectorXd
&
v
,
Eigen
::
VectorXd
&
result
)
{
jmodel
.
jointConfigSelector
(
result
)
=
jmodel
.
integrate
(
q
,
v
);
}
...
...
src/multibody/constraint.hpp
View file @
ffe8faa7
...
...
@@ -181,6 +181,11 @@ namespace se3
return
(
m
.
toActionMatrix
()
*
S
).
eval
();
}
DenseBase
se3ActionInverse
(
const
SE3
&
m
)
const
{
return
(
m
.
inverse
().
toActionMatrix
()
*
S
).
eval
();
}
void
disp_impl
(
std
::
ostream
&
os
)
const
{
os
<<
"S =
\n
"
<<
S
<<
std
::
endl
;}
protected:
...
...
src/multibody/joint/joint-base.hpp
View file @
ffe8faa7
...
...
@@ -399,7 +399,9 @@ namespace se3
int
idx_v
()
const
{
return
i_v
;
}
JointIndex
id
()
const
{
return
i_id
;
}
void
setIndexes
(
JointIndex
id
,
int
q
,
int
v
)
{
i_id
=
id
,
i_q
=
q
;
i_v
=
v
;
}
void
setIndexes
(
JointIndex
id
,
int
q
,
int
v
)
{
derived
().
setIndexes_impl
(
id
,
q
,
v
);
}
void
setIndexes_impl
(
JointIndex
id
,
int
q
,
int
v
)
{
i_id
=
id
,
i_q
=
q
;
i_v
=
v
;
}
void
disp
(
std
::
ostream
&
os
)
const
{
...
...
src/multibody/joint/joint-basic-visitors.hpp
View file @
ffe8faa7
...
...
@@ -19,9 +19,9 @@
#define __se3_joint_basic_visitors_hpp__
#include <Eigen/StdVector>
#include <boost/variant.hpp>
#include "pinocchio/multibody/joint/joint-variant.hpp"
namespace
se3
{
...
...
@@ -108,6 +108,16 @@ namespace se3
const
double
u
);
/**
* @brief Visit a JointModelVariant through JointRandomVisitor to
* generate a random configuration vector
*
* @param[in] jmodel The JointModelVariant
*
* @return The joint randomconfiguration
*/
inline
Eigen
::
VectorXd
random
(
const
JointModelVariant
&
jmodel
);
/**
* @brief Visit a JointModelVariant through JointRandomConfigurationVisitor to
* generate a configuration vector uniformly sampled among provided limits
...
...
@@ -343,7 +353,8 @@ namespace se3
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
// Included later
// #include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
#endif // ifndef __se3_joint_basic_visitors_hpp__
src/multibody/joint/joint-basic-visitors.hxx
View file @
ffe8faa7
...
...
@@ -19,7 +19,8 @@
#define __se3_joint_basic_visitors_hxx__
#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
#include "pinocchio/multibody/joint/joint-composite.hpp"
#include "pinocchio/multibody/visitor.hpp"
namespace
se3
...
...
@@ -192,6 +193,25 @@ namespace se3
return
JointInterpolateVisitor
::
run
(
jmodel
,
q0
,
q1
,
u
);
}
/**
* @brief JointRandomVisitor visitor
*/
class
JointRandomVisitor
:
public
boost
::
static_visitor
<
Eigen
::
VectorXd
>
{
public:
template
<
typename
D
>
Eigen
::
VectorXd
operator
()(
const
JointModelBase
<
D
>
&
jmodel
)
const
{
return
jmodel
.
random
();
}
static
Eigen
::
VectorXd
run
(
const
JointModelVariant
&
jmodel
)
{
return
boost
::
apply_visitor
(
JointRandomVisitor
(),
jmodel
);
}
};
inline
Eigen
::
VectorXd
random
(
const
JointModelVariant
&
jmodel
)
{
return
JointRandomVisitor
::
run
(
jmodel
);
}
/**
* @brief JointRandomConfigurationVisitor visitor
*/
...
...
@@ -348,8 +368,8 @@ namespace se3
{
public:
template
<
typename
D
>
int
operator
()(
const
JointModelBase
<
D
>
&
)
const
{
return
D
::
NV
;
}
int
operator
()(
const
JointModelBase
<
D
>
&
jmodel
)
const
{
return
jmodel
.
nv
()
;
}
static
int
run
(
const
JointModelVariant
&
jmodel
)
{
return
boost
::
apply_visitor
(
JointNvVisitor
(),
jmodel
);
}
...
...
@@ -364,8 +384,8 @@ namespace se3
{
public:
template
<
typename
D
>
int
operator
()(
const
JointModelBase
<
D
>
&
)
const
{
return
D
::
NQ
;
}
int
operator
()(
const
JointModelBase
<
D
>
&
jmodel
)
const
{
return
jmodel
.
nq
()
;
}
static
int
run
(
const
JointModelVariant
&
jmodel
)
{
return
boost
::
apply_visitor
(
JointNqVisitor
(),
jmodel
);
}
...
...
src/multibody/joint/joint-composite.hpp
0 → 100644
View file @
ffe8faa7
//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terljMj of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_joint_composite_hpp__
#define __se3_joint_composite_hpp__
#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/joint/joint.hpp"
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION
(
se3
::
SE3
)
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION
(
se3
::
Motion
)
namespace
se3
{
struct
JointComposite
;
struct
JointModelComposite
;
struct
JointDataComposite
;
template
<
>
struct
traits
<
JointComposite
>
{
enum
{
NQ
=
Eigen
::
Dynamic
,
NV
=
Eigen
::
Dynamic
};
typedef
double
Scalar
;
typedef
JointDataComposite
JointDataDerived
;
typedef
JointModelComposite
JointModelDerived
;
typedef
ConstraintXd
Constraint_t
;
typedef
SE3
Transformation_t
;
typedef
Motion
Motion_t
;
typedef
Motion
Bias_t
;
typedef
Eigen
::
Matrix
<
double
,
6
,
Eigen
::
Dynamic
>
F_t
;
// [ABA]
typedef
Eigen
::
Matrix
<
double
,
6
,
Eigen
::
Dynamic
>
U_t
;
typedef
Eigen
::
Matrix
<
double
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
D_t
;
typedef
Eigen
::
Matrix
<
double
,
6
,
Eigen
::
Dynamic
>
UD_t
;
typedef
Eigen
::
Matrix
<
double
,
Eigen
::
Dynamic
,
1
>
ConfigVector_t
;
typedef
Eigen
::
Matrix
<
double
,
Eigen
::
Dynamic
,
1
>
TangentVector_t
;
};
template
<
>
struct
traits
<
JointDataComposite
>
{
typedef
JointComposite
JointDerived
;
};
template
<
>
struct
traits
<
JointModelComposite
>
{
typedef
JointComposite
JointDerived
;
};
struct
JointDataComposite
:
public
JointDataBase
<
JointDataComposite
>
{
typedef
JointComposite
Joint
;
SE3_JOINT_TYPEDEF
;
JointDataVector
joints
;
int
nq_composite
,
nv_composite
;
Constraint_t
S
;
std
::
vector
<
Transformation_t
>
ljMj
;
Transformation_t
M
;
Motion_t
v
;
Bias_t
c
;
// // [ABA] specific data
U_t
U
;
D_t
Dinv
;
UD_t
UDinv
;
// JointDataComposite() {} // can become necessary if we want a vector of JointDataComposite ?
JointDataComposite
(
JointDataVector
&
joints
,
int
nq
,
int
nv
)
:
joints
(
joints
)
,
nq_composite
(
nq
)
,
nv_composite
(
nv
)
,
S
(
Eigen
::
MatrixXd
::
Zero
(
6
,
nv_composite
))
,
ljMj
(
joints
.
size
())
,
M
(
Transformation_t
::
Identity
())
,
v
(
Motion_t
::
Zero
())
,
c
(
Bias_t
::
Zero
())
,
U
(
Eigen
::
MatrixXd
::
Zero
(
6
,
nv_composite
))
,
Dinv
(
Eigen
::
MatrixXd
::
Zero
(
nv_composite
,
nv_composite
))
,
UDinv
(
Eigen
::
MatrixXd
::
Zero
(
6
,
nv_composite
))
{}
};
struct
JointModelComposite
:
public
JointModelBase
<
JointModelComposite
>
{
typedef
JointComposite
Joint
;
SE3_JOINT_TYPEDEF
;
SE3_JOINT_USE_INDEXES
;
using
JointModelBase
<
JointModelComposite
>::
id
;
using
JointModelBase
<
JointModelComposite
>::
setIndexes
;
std
::
size_t
max_joints
;
JointModelVector
joints
;
int
nq_composite
,
nv_composite
;
// Same as JointModelComposite(1)
JointModelComposite
()
:
max_joints
(
1
)
,
joints
(
0
)
,
nq_composite
(
0
)
,
nv_composite
(
0
)
{}
JointModelComposite
(
std
::
size_t
max_number_of_joints
)
:
max_joints
(
max_number_of_joints
)
,
joints
(
0
)
,
nq_composite
(
0
)
,
nv_composite
(
0
)
{}
template
<
typename
D1
>
JointModelComposite
(
const
JointModelBase
<
D1
>
&
jmodel1
)
:
max_joints
(
1
)
,
joints
(
0
)
,
nq_composite
(
jmodel1
.
nq
())
,
nv_composite
(
jmodel1
.
nv
())
{
joints
.
push_back
(
JointModel
(
jmodel1
.
derived
()));
}
template
<
typename
D1
,
typename
D2
>
JointModelComposite
(
const
JointModelBase
<
D1
>
&
jmodel1
,
const
JointModelBase
<
D2
>
&
jmodel2
)
:
max_joints
(
2
)
,
joints
(
0
)
,
nq_composite
(
jmodel1
.
nq
()
+
jmodel2
.
nq
())
,
nv_composite
(
jmodel1
.
nv
()
+
jmodel2
.
nv
())
{
joints
.
push_back
(
JointModel
(
jmodel1
.
derived
()));
joints
.
push_back
(
JointModel
(
jmodel2
.
derived
()));
}
template
<
typename
D1
,
typename
D2
,
typename
D3
>
JointModelComposite
(
const
JointModelBase
<
D1
>
&
jmodel1
,
const
JointModelBase
<
D2
>
&
jmodel2
,
const
JointModelBase
<
D3
>
&
jmodel3
)
:
max_joints
(
3
)
,
joints
(
0
)
,
nq_composite
(
jmodel1
.
nq
()
+
jmodel2
.
nq
()
+
jmodel3
.
nq
())
,
nv_composite
(
jmodel1
.
nv
()
+
jmodel2
.
nv
()
+
jmodel3
.
nv
())
{
joints
.
push_back
(
JointModel
(
jmodel1
.
derived
()));
joints
.
push_back
(
JointModel
(
jmodel2
.
derived
()));
joints
.
push_back
(
JointModel
(
jmodel3
.
derived
()));
}
// JointModelComposite( const JointModelVector & models ) : max_joints(models.size()) , joints(models) {}
template
<
typename
D
>
std
::
size_t
addJointModel
(
const
JointModelBase
<
D
>
&
jmodel
)
{
std
::
size_t
nb_joints
=
joints
.
size
();
if
(
!
isFull
())
{
joints
.
push_back
(
JointModel
(
jmodel
.
derived
()));
nq_composite
+=
jmodel
.
nq
();
nv_composite
+=
jmodel
.
nv
();
nb_joints
=
joints
.
size
();
}
return
nb_joints
;
}
bool
isFull
()
const
{
return
joints
.
size
()
==
max_joints
;
}
bool
isEmpty
()
const
{
return
joints
.
size
()
<=
0
;
}
JointDataDerived
createData
()
const
{
JointDataVector
res
;
for
(
JointModelVector
::
const_iterator
i
=
joints
.
begin
();
i
!=
joints
.
end
();
++
i
)
{
res
.
push_back
(
::
se3
::
createData
(
*
i
));
}
return
JointDataDerived
(
res
,
nq_composite
,
nv_composite
);
}
void
calc
(
JointDataDerived
&
data
,
const
Eigen
::
VectorXd
&
qs
)
const
{
data
.
M
.
setIdentity
();
for
(
JointDataVector
::
iterator
i
=
data
.
joints
.
begin
();
i
!=
data
.
joints
.
end
();
++
i
)
{
JointDataVector
::
iterator
::
difference_type
index
=
i
-
data
.
joints
.
begin
();
calc_zero_order
(
joints
[(
std
::
size_t
)
index
],
*
i
,
qs
);
data
.
ljMj
[(
std
::
size_t
)
index
]
=
joint_transform
(
*
i
);
data
.
M
=
data
.
M
*
data
.
ljMj
[(
std
::
size_t
)
index
];
}
}
void
calc
(
JointDataDerived
&
data
,
const
Eigen
::
VectorXd
&
qs
,
const
Eigen
::
VectorXd
&
vs
)
const
{
data
.
M
.
setIdentity
();
data
.
v
.
setZero
();
data
.
c
.
setZero
();
data
.
S
.
matrix
().
setZero
();
for
(
JointDataVector
::
iterator
i
=
data
.
joints
.
begin
();
i
!=
data
.
joints
.
end
();
++
i
)
{
JointDataVector
::
iterator
::
difference_type
index
=
i
-
data
.
joints
.
begin
();
calc_first_order
(
joints
[(
std
::
size_t
)
index
],
*
i
,
qs
,
vs
);
data
.
ljMj
[(
std
::
size_t
)
index
]
=
joint_transform
(
*
i
);
data
.
M
=
data
.
M
*
data
.
ljMj
[(
std
::
size_t
)
index
];
data
.
v
=
motion
(
*
i
);
if
(
i
==
data
.
joints
.
begin
())
{}
else
{
data
.
v
+=
data
.
ljMj
[(
std
::
size_t
)
index
].
actInv
(
motion
(
*
(
i
-
1
)));
}
}
data
.
c
=
bias
(
data
.
joints
[
joints
.
size
()
-
1
]);
int
start_col
=
nv_composite
;
int
sub_constraint_dimension
=
(
int
)
constraint_xd
(
data
.
joints
[
joints
.
size
()
-
1
]).
matrix
().
cols
();
start_col
-=
sub_constraint_dimension
;
data
.
S
.
matrix
().
middleCols
(
start_col
,
sub_constraint_dimension
)
=
constraint_xd
(
data
.
joints
[
joints
.
size
()
-
1
]).
matrix
();
SE3
iMcomposite
(
SE3
::
Identity
());
for
(
JointDataVector
::
reverse_iterator
i
=
data
.
joints
.
rbegin
()
+
1
;
i
!=
data
.
joints
.
rend
();
++
i
)
{
sub_constraint_dimension
=
(
int
)
constraint_xd
(
*
i
).
matrix
().
cols
();
start_col
-=
sub_constraint_dimension
;
iMcomposite
=
joint_transform
(
*
(
i
+
0
))
*
iMcomposite
;
data
.
S
.
matrix
().
middleCols
(
start_col
,
sub_constraint_dimension
)
=
iMcomposite
.
actInv
(
constraint_xd
(
*
(
i
+
0
)));
Motion
acceleration_d_entrainement_coriolis
=
Motion
::
Zero
();
// TODO: compute
data
.
c
+=
iMcomposite
.
actInv
(
bias
(
*
i
))
+
acceleration_d_entrainement_coriolis
;
}
}
void
calc_aba
(
JointDataDerived
&
data
,
Inertia
::
Matrix6
&
I
,
const
bool
update_I
)
const
{
// calc has been called previously in abaforwardstep1 so data.M, data.v are up to date
data
.
U
.
setZero
();
data
.
Dinv
.
setZero
();
data
.
UDinv
.
setZero
();
for
(
JointDataVector
::
iterator
i
=
data
.
joints
.
begin
();
i
!=
data
.
joints
.
end
();
++
i
)
{
JointDataVector
::
iterator
::
difference_type
index
=
i
-
data
.
joints
.
begin
();
::
se3
::
calc_aba
(
joints
[(
std
::
size_t
)
index
],
*
i
,
I
,
false
);
}
data
.
U
=
I
*
data
.
S
;
Eigen
::
MatrixXd
tmp
=
data
.
S
.
matrix
().
transpose
()
*
I
*
data
.
S
.
matrix
();
data
.
Dinv
=
tmp
.
inverse
();
data
.
UDinv
=
data
.
U
*
data
.
Dinv
;
if
(
update_I
)
I
-=
data
.
UDinv
*
data
.
U
.
transpose
();
}
Scalar
finiteDifferenceIncrement
()
const
{
using
std
::
sqrt
;
return
2.
*
sqrt
(
sqrt
(
Eigen
::
NumTraits
<
Scalar
>::
epsilon
()));
}
ConfigVector_t
integrate_impl
(
const
Eigen
::
VectorXd
&
qs
,
const
Eigen
::
VectorXd
&
vs
)
const
{
ConfigVector_t
result
(
Eigen
::
VectorXd
::
Zero
(
nq_composite
));
for
(
JointModelVector
::
const_iterator
i
=
joints
.
begin
();
i
!=
joints
.
end
();
++
i
)
{
result
.
segment
(
::
se3
::
idx_q
(
*
i
),
::
se3
::
nq
(
*
i
))
+=
::
se3
::
integrate
(
*
i
,
qs
,
vs
);
}
return
result
;
}
ConfigVector_t
interpolate_impl
(
const
Eigen
::
VectorXd
&
q0
,
const
Eigen
::
VectorXd
&
q1
,
const
double
u
)
const
{
ConfigVector_t
result
(
Eigen
::
VectorXd
::
Zero
(
nq_composite
));
for
(
JointModelVector
::
const_iterator
i
=
joints
.
begin
();
i
!=
joints
.
end
();
++
i
)
{
result
.
segment
(
::
se3
::
idx_q
(
*
i
),
::
se3
::
nq
(
*
i
))
+=
::
se3
::
interpolate
(
*
i
,
q0
,
q1
,
u
);
}
return
result
;
}
ConfigVector_t
random_impl
()
const
{
ConfigVector_t
result
(
Eigen
::
VectorXd
::
Zero
(
nq_composite
));
for
(
JointModelVector
::
const_iterator
i
=
joints
.
begin
();
i
!=
joints
.
end
();
++
i
)
{
result
.
segment
(
::
se3
::
idx_q
(
*
i
),
::
se3
::
nq
(
*
i
))
+=
::
se3
::
random
(
*
i
);
}
return
result
;
}
ConfigVector_t
randomConfiguration_impl
(
const
ConfigVector_t
&
lower_pos_limit
,
const
ConfigVector_t
&
upper_pos_limit
)
const
throw
(
std
::
runtime_error
)
{
ConfigVector_t
result
(
Eigen
::
VectorXd
::
Zero
(
nq_composite
));
for
(
JointModelVector
::
const_iterator
i
=
joints
.
begin
();
i
!=
joints
.
end
();
++
i
)
{
result
.
segment
(
::
se3
::
idx_q
(
*
i
),
::
se3
::
nq
(
*
i
))
+=
::
se3
::
randomConfiguration
(
*
i
,
lower_pos_limit
.
segment
(
::
se3
::
idx_q
(
*
i
),
::
se3
::
nq
(
*
i
)),
upper_pos_limit
.
segment
(
::
se3
::
idx_q
(
*
i
),
::
se3
::
nq
(
*
i
))
);
}
return
result
;
}
TangentVector_t
difference_impl
(
const
Eigen
::
VectorXd
&
q0
,
const
Eigen
::
VectorXd
&
q1
)
const
{
TangentVector_t
result
(
Eigen
::
VectorXd
::
Zero
(
nv_composite
));
for
(
JointModelVector
::
const_iterator
i
=
joints
.
begin
();
i
!=
joints
.
end
();
++
i
)
{
result
.
segment
(
::
se3
::
idx_v
(
*
i
),
::
se3
::
nv
(
*
i
))
+=
::
se3
::
difference
(
*
i
,
q0
,
q1
);
}
return
result
;
}
double
distance_impl
(
const
Eigen
::
VectorXd
&
q0
,
const
Eigen
::
VectorXd
&
q1
)
const
{
return
difference_impl
(
q0
,
q1
).
norm
();
}
void
normalize_impl
(
Eigen
::
VectorXd
&
q
)
const
{
for
(
JointModelVector
::
const_iterator
i
=
joints
.
begin
();
i
!=
joints
.
end
();
++
i
)
{
::
se3
::
normalize
(
*
i
,
q
);
}
}
ConfigVector_t
neutralConfiguration_impl
()
const
{
ConfigVector_t
result
(
Eigen
::
VectorXd
::
Zero
(
nq_composite
));
for
(
JointModelVector
::
const_iterator
i
=
joints
.
begin
();
i
!=
joints
.
end
();
++
i
)
{
result
.
segment
(
::
se3
::
idx_q
(
*
i
),
::
se3
::
nq
(
*
i
))
+=
::
se3
::
neutralConfiguration
(
*
i
);
}
return
result
;
}
bool
isSameConfiguration_impl
(
const
Eigen
::
VectorXd
&
q1
,
const