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
65b9bf19
Commit
65b9bf19
authored
Jul 24, 2016
by
jcarpent
Browse files
Minor clean of the code
parent
77955e24
Changes
12
Hide whitespace changes
Inline
Side-by-side
src/multibody/joint/joint-free-flyer.hpp
View file @
65b9bf19
...
...
@@ -379,12 +379,9 @@ namespace se3
q
.
segment
<
4
>
(
idx_q
()
+
3
).
normalize
();
}
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
{
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
()
);
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
());
}
static
std
::
string
classname
()
{
return
std
::
string
(
"JointModelFreeFlyer"
);
}
...
...
src/multibody/joint/joint-planar.hpp
View file @
65b9bf19
...
...
@@ -434,10 +434,7 @@ namespace se3
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
{
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
()
);
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
());
}
static
std
::
string
classname
()
{
return
std
::
string
(
"JointModelPlanar"
);}
...
...
src/multibody/joint/joint-prismatic-unaligned.hpp
View file @
65b9bf19
...
...
@@ -438,12 +438,9 @@ namespace se3
return
q
;
}
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
{
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
()
);
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
());
}
static
std
::
string
classname
()
{
return
std
::
string
(
"JointModelPrismaticUnaligned"
);
}
...
...
src/multibody/joint/joint-prismatic.hpp
View file @
65b9bf19
...
...
@@ -493,12 +493,9 @@ namespace se3
return
q
;
}
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
{
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
()
);
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
());
}
static
std
::
string
classname
();
...
...
src/multibody/joint/joint-revolute-unaligned.hpp
View file @
65b9bf19
...
...
@@ -442,12 +442,9 @@ namespace se3
return
q
;
}
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
{
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
()
);
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
());
}
static
std
::
string
classname
()
{
return
std
::
string
(
"JointModelRevoluteUnaligned"
);
}
...
...
src/multibody/joint/joint-revolute-unbounded.hpp
View file @
65b9bf19
...
...
@@ -243,12 +243,9 @@ namespace se3
q
.
segment
<
NQ
>
(
idx_q
()).
normalize
();
}
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
{
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
()
);
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
());
}
static
std
::
string
classname
();
...
...
src/multibody/joint/joint-revolute.hpp
View file @
65b9bf19
...
...
@@ -527,12 +527,9 @@ namespace se3
return
q
;
}
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
{
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
()
);
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
());
}
static
std
::
string
classname
();
...
...
src/multibody/joint/joint-spherical-ZYX.hpp
View file @
65b9bf19
...
...
@@ -459,12 +459,9 @@ namespace se3
return
q
;
}
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
{
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
()
);
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
());
}
static
std
::
string
classname
()
{
return
std
::
string
(
"JointModelSphericalZYX"
);
}
...
...
src/multibody/joint/joint-spherical.hpp
View file @
65b9bf19
...
...
@@ -404,12 +404,9 @@ namespace se3
q
.
segment
<
NQ
>
(
idx_q
()).
normalize
();
}
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
{
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
()
);
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
());
}
static
std
::
string
classname
()
{
return
std
::
string
(
"JointModelSpherical"
);
}
...
...
src/multibody/joint/joint-translation.hpp
View file @
65b9bf19
...
...
@@ -356,12 +356,12 @@ namespace se3
Eigen
::
VectorXd
::
ConstFixedSegmentReturnType
<
NQ
>::
Type
&
q_0
=
q0
.
segment
<
NQ
>
(
idx_q
());
Eigen
::
VectorXd
::
ConstFixedSegmentReturnType
<
NQ
>::
Type
&
q_1
=
q1
.
segment
<
NQ
>
(
idx_q
());
return
(
q_1
-
q_0
)
;
return
q_1
-
q_0
;
}
double
distance_impl
(
const
Eigen
::
VectorXd
&
q0
,
const
Eigen
::
VectorXd
&
q1
)
const
{
return
difference_impl
(
q0
,
q1
).
norm
();
return
difference_impl
(
q0
,
q1
).
norm
();
}
ConfigVector_t
neutralConfiguration_impl
()
const
...
...
@@ -371,12 +371,9 @@ namespace se3
return
q
;
}
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
JointModelDense
<
NQ
,
NV
>
toDense_impl
()
const
{
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
()
);
return
JointModelDense
<
NQ
,
NV
>
(
id
(),
idx_q
(),
idx_v
());
}
static
std
::
string
classname
()
{
return
std
::
string
(
"JointModelTranslation"
);
}
...
...
src/multibody/joint/joint.hpp
View file @
65b9bf19
...
...
@@ -32,7 +32,6 @@ namespace se3
template
<
>
struct
traits
<
Joint
>
{
enum
{
NQ
=
-
1
,
// Dynamic because unknown at compilation
NV
=
-
1
...
...
@@ -77,9 +76,8 @@ namespace se3
const
D_t
Dinv
()
const
{
return
dinv_inertia
(
*
this
);
}
const
UD_t
UDinv
()
const
{
return
udinv_inertia
(
*
this
);
}
JointData
()
:
JointDataBoostVariant
()
{}
JointData
(
const
JointDataVariant
&
jdata
)
:
JointDataBoostVariant
(
jdata
){}
JointData
(
const
JointDataVariant
&
jdata
)
:
JointDataBoostVariant
(
jdata
)
{}
};
...
...
@@ -94,62 +92,55 @@ namespace se3
using
Base
::
setIndexes
;
using
Base
::
operator
==
;
JointModel
()
:
JointModelBoostVariant
()
{}
JointModel
(
const
JointModelVariant
&
model_variant
)
:
JointModelBoostVariant
(
model_variant
)
{}
JointModelVariant
&
toVariant
()
{
return
*
static_cast
<
JointModelVariant
*>
(
this
);
}
const
JointModelVariant
&
toVariant
()
const
{
return
*
static_cast
<
const
JointModelVariant
*>
(
this
);
}
JointDataVariant
createData
()
{
return
::
se3
::
createData
(
*
this
);
}
JointDataVariant
createData
()
{
return
::
se3
::
createData
(
*
this
);
}
void
calc
(
JointData
&
data
,
const
Eigen
::
VectorXd
&
q
)
const
{
calc_zero_order
(
*
this
,
data
,
q
);
}
void
calc
(
JointData
&
data
,
const
Eigen
::
VectorXd
&
qs
)
const
void
calc
(
JointData
&
data
,
const
Eigen
::
VectorXd
&
q
,
const
Eigen
::
VectorXd
&
v
)
const
{
calc_zero_order
(
*
this
,
data
,
qs
);
}
void
calc
(
JointData
&
data
,
const
Eigen
::
VectorXd
&
qs
,
const
Eigen
::
VectorXd
&
vs
)
const
{
calc_first_order
(
*
this
,
data
,
qs
,
vs
);
calc_first_order
(
*
this
,
data
,
q
,
v
);
}
void
calc_aba
(
JointData
&
data
,
Inertia
::
Matrix6
&
I
,
const
bool
update_I
)
const
{
::
se3
::
calc_aba
(
*
this
,
data
,
I
,
update_I
);
::
se3
::
calc_aba
(
*
this
,
data
,
I
,
update_I
);
}
ConfigVector_t
integrate_impl
(
const
Eigen
::
VectorXd
&
q
s
,
const
Eigen
::
VectorXd
&
v
s
)
const
ConfigVector_t
integrate_impl
(
const
Eigen
::
VectorXd
&
q
,
const
Eigen
::
VectorXd
&
v
)
const
{
return
::
se3
::
integrate
(
*
this
,
qs
,
vs
);
return
::
se3
::
integrate
(
*
this
,
q
,
v
);
}
ConfigVector_t
interpolate_impl
(
const
Eigen
::
VectorXd
&
q0
,
const
Eigen
::
VectorXd
&
q1
,
const
double
u
)
const
{
return
::
se3
::
interpolate
(
*
this
,
q0
,
q1
,
u
);
return
::
se3
::
interpolate
(
*
this
,
q0
,
q1
,
u
);
}
ConfigVector_t
randomConfiguration_impl
(
const
ConfigVector_t
&
lower_pos_limit
,
const
ConfigVector_t
&
upper_pos_limit
)
const
throw
(
std
::
runtime_error
)
ConfigVector_t
randomConfiguration_impl
(
const
ConfigVector_t
&
lower_pos_limit
,
const
ConfigVector_t
&
upper_pos_limit
)
const
throw
(
std
::
runtime_error
)
{
return
::
se3
::
randomConfiguration
(
*
this
,
lower_pos_limit
,
upper_pos_limit
);
return
::
se3
::
randomConfiguration
(
*
this
,
lower_pos_limit
,
upper_pos_limit
);
}
TangentVector_t
difference_impl
(
const
Eigen
::
VectorXd
&
q0
,
const
Eigen
::
VectorXd
&
q1
)
const
{
return
::
se3
::
difference
(
*
this
,
q0
,
q1
);
return
::
se3
::
difference
(
*
this
,
q0
,
q1
);
}
double
distance_impl
(
const
Eigen
::
VectorXd
&
q0
,
const
Eigen
::
VectorXd
&
q1
)
const
{
return
::
se3
::
distance
(
*
this
,
q0
,
q1
);
return
::
se3
::
distance
(
*
this
,
q0
,
q1
);
}
void
normalize_impl
(
Eigen
::
VectorXd
&
q
)
const
{
return
::
se3
::
normalize
(
*
this
,
q
);
return
::
se3
::
normalize
(
*
this
,
q
);
}
ConfigVector_t
neutralConfiguration_impl
()
const
...
...
@@ -168,11 +159,7 @@ namespace se3
JointIndex
id
()
const
{
return
::
se3
::
id
(
*
this
);
}
// void setIndexes(JointIndex ,int ,int ) { SE3_STATIC_ASSERT(false, THIS_METHOD_SHOULD_NOT_BE_CALLED_ON_DERIVED_CLASS); }
void
setIndexes
(
JointIndex
id
,
int
nq
,
int
nv
)
{
::
se3
::
setIndexes
(
*
this
,
id
,
nq
,
nv
);
}
void
setIndexes
(
JointIndex
id
,
int
nq
,
int
nv
)
{
::
se3
::
setIndexes
(
*
this
,
id
,
nq
,
nv
);
}
};
typedef
std
::
vector
<
JointData
>
JointDataVector
;
...
...
src/python/joints-variant.hpp
View file @
65b9bf19
//
// Copyright (c) 2015 CNRS
// Copyright (c) 2015
-2016
CNRS
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
...
...
@@ -23,8 +23,6 @@
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/python/joints-models.hpp"
namespace
se3
{
namespace
python
...
...
@@ -33,45 +31,37 @@ namespace se3
struct
jointModelVariantVisitor
:
boost
::
static_visitor
<
PyObject
*>
{
static
result_type
convert
(
JointModelVariant
const
&
jv
)
static
result_type
convert
(
JointModelVariant
const
&
jv
)
{
return
apply_visitor
(
jointModelVariantVisitor
(),
jv
);
}
template
<
typename
T
>
result_type
operator
()(
T
const
&
t
)
const
result_type
operator
()(
T
const
&
t
)
const
{
return
boost
::
python
::
incref
(
boost
::
python
::
object
(
t
).
ptr
());
}
};
struct
exposer
{
template
<
class
T
>
inline
void
operator
()(
T
)
struct
exposer
{
template
<
class
T
>
void
operator
()(
T
)
{
expose_constructors
<
T
>
(
bp
::
class_
<
T
>
(
T
::
classname
().
c_str
(),
bp
::
init
<>
()).
def
(
JointPythonVisitor
<
T
>
()));
bp
::
implicitly_convertible
<
T
,
se3
::
JointModelVariant
>
();
}
};
// For the moment, only expose models of joint. Not data ( to do it, split exposer into exposerModels & exposer_Datas and do another for_each)
static
void
exposeVariants
()
{
boost
::
mpl
::
for_each
<
JointModelVariant
::
types
>
(
exposer
());
bp
::
to_python_converter
<
se3
::
JointModelVariant
,
jointModelVariantVisitor
>
();
// bp::def("make_variant", se3::make_variant);
}
}}
// namespace se3::python
}
// namespace python
}
// namespace se3
#endif // ifndef __se3_python_joints_variant_hpp__
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