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
4a28d1e7
Commit
4a28d1e7
authored
Oct 17, 2019
by
Wolfgang Merkt
Browse files
Revert assert changes to multibody and codegen
parent
ac267812
Changes
26
Hide whitespace changes
Inline
Side-by-side
src/codegen/code-generator-algo.hpp
View file @
4a28d1e7
...
...
@@ -256,7 +256,7 @@ namespace pinocchio
}
}
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
it_Y
==
Base
::
getOutputDimension
());
assert
(
it_Y
==
Base
::
getOutputDimension
());
ad_fun
.
Dependent
(
ad_X
,
ad_Y
);
ad_fun
.
optimize
(
"no_compare_op"
);
...
...
@@ -281,7 +281,7 @@ namespace pinocchio
}
}
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
it_Y
==
Base
::
getOutputDimension
());
assert
(
it_Y
==
Base
::
getOutputDimension
());
}
protected:
...
...
@@ -343,7 +343,7 @@ namespace pinocchio
}
}
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
it_Y
==
Base
::
getOutputDimension
());
assert
(
it_Y
==
Base
::
getOutputDimension
());
ad_fun
.
Dependent
(
ad_X
,
ad_Y
);
ad_fun
.
optimize
(
"no_compare_op"
);
...
...
@@ -438,7 +438,7 @@ namespace pinocchio
ad_q
,
ad_v
,
ad_a
,
ad_dtau_dq
,
ad_dtau_dv
,
ad_dtau_da
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
ad_Y
.
size
()
==
Base
::
getOutputDimension
());
assert
(
ad_Y
.
size
()
==
Base
::
getOutputDimension
());
Eigen
::
DenseIndex
it_Y
=
0
;
Eigen
::
Map
<
RowADMatrixXs
>
(
ad_Y
.
data
()
+
it_Y
,
ad_model
.
nv
,
ad_model
.
nv
)
=
ad_dtau_dq
;
...
...
@@ -547,7 +547,7 @@ namespace pinocchio
ad_q
,
ad_v
,
ad_tau
,
ad_dddq_dq
,
ad_dddq_dv
,
ad_dddq_dtau
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
ad_Y
.
size
()
==
Base
::
getOutputDimension
());
assert
(
ad_Y
.
size
()
==
Base
::
getOutputDimension
());
Eigen
::
DenseIndex
it_Y
=
0
;
Eigen
::
Map
<
RowADMatrixXs
>
(
ad_Y
.
data
()
+
it_Y
,
ad_model
.
nv
,
ad_model
.
nv
)
=
ad_dddq_dq
;
...
...
src/codegen/code-generator-base.hpp
View file @
4a28d1e7
...
...
@@ -122,7 +122,7 @@ namespace pinocchio
template
<
typename
Vector
>
void
evalFunction
(
const
Eigen
::
MatrixBase
<
Vector
>
&
x
)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
build_forward
);
assert
(
build_forward
);
generatedFun_ptr
->
ForwardZero
(
PINOCCHIO_EIGEN_CONST_CAST
(
Vector
,
x
),
y
);
}
...
...
@@ -130,7 +130,7 @@ namespace pinocchio
template
<
typename
Vector
>
void
evalJacobian
(
const
Eigen
::
MatrixBase
<
Vector
>
&
x
)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
build_jacobian
);
assert
(
build_jacobian
);
CppAD
::
cg
::
ArrayView
<
const
Scalar
>
x_
(
PINOCCHIO_EIGEN_CONST_CAST
(
Vector
,
x
).
data
(),(
size_t
)
x
.
size
());
CppAD
::
cg
::
ArrayView
<
Scalar
>
jac_
(
jac
.
data
(),(
size_t
)
jac
.
size
());
...
...
src/multibody/data.hxx
View file @
4a28d1e7
...
...
@@ -166,7 +166,7 @@ namespace pinocchio
const
int
nvj
=
nv
(
model
.
joints
[
joint
]);
const
int
idx_vj
=
idx_v
(
model
.
joints
[
joint
]);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
idx_vj
>=
0
&&
idx_vj
<
model
.
nv
);
assert
(
idx_vj
>=
0
&&
idx_vj
<
model
.
nv
);
if
(
parent
>
0
)
parents_fromRow
[(
Index
)
idx_vj
]
=
idx_v
(
model
.
joints
[
parent
])
+
nv
(
model
.
joints
[
parent
])
-
1
;
else
parents_fromRow
[(
Index
)
idx_vj
]
=
-
1
;
...
...
src/multibody/fcl.hxx
View file @
4a28d1e7
...
...
@@ -20,7 +20,7 @@ namespace pinocchio
inline
CollisionPair
::
CollisionPair
(
const
GeomIndex
co1
,
const
GeomIndex
co2
)
:
Base
(
co1
,
co2
)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE
(
co1
!=
co2
,
"The index of collision objects must not be equal."
);
assert
(
co1
!=
co2
&&
"The index of collision objects must not be equal."
);
}
inline
bool
CollisionPair
::
operator
==
(
const
CollisionPair
&
rhs
)
const
...
...
src/multibody/force-set.hpp
View file @
4a28d1e7
...
...
@@ -31,7 +31,7 @@ namespace pinocchio
{
m_f
.
fill
(
NAN
);
m_n
.
fill
(
NAN
);
}
ForceSetTpl
(
const
Matrix3x
&
linear
,
const
Matrix3x
&
angular
)
:
size
((
int
)
linear
.
cols
()),
m_f
(
linear
),
m_n
(
angular
)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
linear
.
cols
()
==
angular
.
cols
()
);
}
{
assert
(
linear
.
cols
()
==
angular
.
cols
()
);
}
Matrix6x
matrix
()
const
{
...
...
@@ -92,7 +92,7 @@ namespace pinocchio
Block
&
operator
=
(
const
ForceSetTpl
&
copy
)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
copy
.
size
==
len
);
assert
(
copy
.
size
==
len
);
linear
()
=
copy
.
linear
();
//ref.m_f.block(0,idx,3,len) = copy.m_f;
angular
()
=
copy
.
angular
();
//ref.m_n.block(0,idx,3,len) = copy.m_n;
return
*
this
;
...
...
@@ -100,7 +100,7 @@ namespace pinocchio
Block
&
operator
=
(
const
ForceSetTpl
::
Block
&
copy
)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
copy
.
len
==
len
);
assert
(
copy
.
len
==
len
);
linear
()
=
copy
.
linear
();
//ref.m_f.block(0,idx,3,len) = copy.ref.m_f.block(0,copy.idx,3,copy.len);
angular
()
=
copy
.
angular
();
//ref.m_n.block(0,idx,3,len) = copy.ref.m_n.block(0,copy.idx,3,copy.len);
return
*
this
;
...
...
@@ -110,7 +110,7 @@ namespace pinocchio
Block
&
operator
=
(
const
Eigen
::
MatrixBase
<
D
>
&
m
)
{
eigen_assert
(
D
::
RowsAtCompileTime
==
6
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
m
.
cols
()
==
len
);
assert
(
m
.
cols
()
==
len
);
linear
()
=
m
.
template
topRows
<
3
>();
angular
()
=
m
.
template
bottomRows
<
3
>();
return
*
this
;
...
...
src/multibody/geometry.hxx
View file @
4a28d1e7
...
...
@@ -42,9 +42,9 @@ namespace pinocchio
GeomIndex
GeometryModel
::
addGeometryObject
(
const
GeometryObject
&
object
,
const
ModelTpl
<
S2
,
O2
,
JointCollectionTpl
>
&
model
)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
//TODO: reenable when relevant (object.parentFrame == -1) ||
assert
(
//TODO: reenable when relevant (object.parentFrame == -1) ||
(
model
.
frames
[
object
.
parentFrame
].
type
==
pinocchio
::
BODY
)
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
//TODO: reenable when relevant (object.parentFrame == -1) ||
assert
(
//TODO: reenable when relevant (object.parentFrame == -1) ||
(
model
.
frames
[
object
.
parentFrame
].
parent
==
object
.
parentJoint
)
);
GeomIndex
idx
=
(
GeomIndex
)
(
ngeoms
++
);
...
...
@@ -82,7 +82,7 @@ namespace pinocchio
inline
const
std
::
string
&
GeometryModel
::
getGeometryName
(
const
GeomIndex
index
)
const
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
index
<
(
GeomIndex
)
geometryObjects
.
size
()
);
assert
(
index
<
(
GeomIndex
)
geometryObjects
.
size
()
);
return
geometryObjects
[
index
].
name
;
}
...
...
@@ -168,7 +168,7 @@ namespace pinocchio
inline
void
GeometryModel
::
addCollisionPair
(
const
CollisionPair
&
pair
)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
(
pair
.
first
<
ngeoms
)
&&
(
pair
.
second
<
ngeoms
)
);
assert
(
(
pair
.
first
<
ngeoms
)
&&
(
pair
.
second
<
ngeoms
)
);
if
(
!
existCollisionPair
(
pair
))
{
collisionPairs
.
push_back
(
pair
);
}
}
...
...
@@ -189,7 +189,7 @@ namespace pinocchio
inline
void
GeometryModel
::
removeCollisionPair
(
const
CollisionPair
&
pair
)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
(
pair
.
first
<
ngeoms
)
&&
(
pair
.
second
<
ngeoms
)
);
assert
(
(
pair
.
first
<
ngeoms
)
&&
(
pair
.
second
<
ngeoms
)
);
CollisionPairVector
::
iterator
it
=
std
::
find
(
collisionPairs
.
begin
(),
collisionPairs
.
end
(),
...
...
@@ -217,19 +217,19 @@ namespace pinocchio
inline
void
GeometryData
::
activateCollisionPair
(
const
PairIndex
pairId
,
const
bool
flag
)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
pairId
<
activeCollisionPairs
.
size
()
);
assert
(
pairId
<
activeCollisionPairs
.
size
()
);
activeCollisionPairs
[
pairId
]
=
flag
;
}
inline
void
GeometryData
::
activateCollisionPair
(
const
PairIndex
pairId
)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
pairId
<
activeCollisionPairs
.
size
()
);
assert
(
pairId
<
activeCollisionPairs
.
size
()
);
activeCollisionPairs
[
pairId
]
=
true
;
}
inline
void
GeometryData
::
deactivateCollisionPair
(
const
PairIndex
pairId
)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
pairId
<
activeCollisionPairs
.
size
()
);
assert
(
pairId
<
activeCollisionPairs
.
size
()
);
activeCollisionPairs
[
pairId
]
=
false
;
}
...
...
src/multibody/joint/joint-common-operations.hpp
View file @
4a28d1e7
...
...
@@ -57,7 +57,7 @@ namespace pinocchio
const
Scalar
&
offset
,
const
Eigen
::
MatrixBase
<
ConfigVectorOut
>
&
dest
)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
q
.
size
()
==
dest
.
size
());
assert
(
q
.
size
()
==
dest
.
size
());
PINOCCHIO_EIGEN_CONST_CAST
(
ConfigVectorOut
,
dest
).
noalias
()
=
scaling
*
q
+
ConfigVectorOut
::
Constant
(
dest
.
size
(),
offset
);
}
};
...
...
src/multibody/joint/joint-composite.hxx
View file @
4a28d1e7
...
...
@@ -58,8 +58,8 @@ namespace pinocchio
inline
void
JointModelCompositeTpl
<
Scalar
,
Options
,
JointCollectionTpl
>::
calc
(
JointDataDerived
&
data
,
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
qs
)
const
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
joints
.
size
()
>
0
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
data
.
joints
.
size
()
==
joints
.
size
());
assert
(
joints
.
size
()
>
0
);
assert
(
data
.
joints
.
size
()
==
joints
.
size
());
typedef
JointCompositeCalcZeroOrderStep
<
Scalar
,
Options
,
JointCollectionTpl
,
ConfigVectorType
>
Algo
;
...
...
@@ -133,8 +133,8 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
ConfigVectorType
>
&
qs
,
const
Eigen
::
MatrixBase
<
TangentVectorType
>
&
vs
)
const
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
joints
.
size
()
>
0
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
jdata
.
joints
.
size
()
==
joints
.
size
());
assert
(
joints
.
size
()
>
0
);
assert
(
jdata
.
joints
.
size
()
==
joints
.
size
());
typedef
JointCompositeCalcFirstOrderStep
<
Scalar
,
Options
,
JointCollectionTpl
,
ConfigVectorType
,
TangentVectorType
>
Algo
;
...
...
src/multibody/joint/joint-free-flyer.hpp
View file @
4a28d1e7
...
...
@@ -218,8 +218,8 @@ namespace pinocchio
typedef
Eigen
::
Map
<
const
Quaternion
>
ConstQuaternionMap
;
ConstQuaternionMap
quat
(
q_joint
.
template
tail
<
4
>().
data
());
//
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
math
::
fabs
(
quat
.
coeffs
().
squaredNorm
()
-
1.
)
<=
1e-4
);
//
assert
(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
assert
(
math
::
fabs
(
quat
.
coeffs
().
squaredNorm
()
-
1.
)
<=
1e-4
);
M
.
rotation
(
quat
.
matrix
());
M
.
translation
(
q_joint
.
template
head
<
3
>());
...
...
src/multibody/joint/joint-mimic.hpp
View file @
4a28d1e7
...
...
@@ -87,7 +87,7 @@ namespace pinocchio
template
<
typename
VectorLike
>
JointMotion
__mult__
(
const
Eigen
::
MatrixBase
<
VectorLike
>
&
v
)
const
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
v
.
size
()
==
nv
());
assert
(
v
.
size
()
==
nv
());
JointMotion
jm
=
m_constraint
*
v
;
return
jm
*
m_scaling_factor
;
}
...
...
src/multibody/joint/joint-planar.hpp
View file @
4a28d1e7
...
...
@@ -239,7 +239,7 @@ namespace pinocchio
{
typedef
typename
Eigen
::
MatrixBase
<
Derived
>::
Scalar
Scalar
;
typedef
Eigen
::
Matrix
<
Scalar
,
3
,
Derived
::
ColsAtCompileTime
>
MatrixReturnType
;
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
F
.
rows
()
==
6
);
assert
(
F
.
rows
()
==
6
);
MatrixReturnType
result
(
3
,
F
.
cols
());
result
.
template
topRows
<
2
>()
=
F
.
template
topRows
<
2
>();
...
...
src/multibody/joint/joint-prismatic-unaligned.hpp
View file @
4a28d1e7
...
...
@@ -471,7 +471,7 @@ namespace pinocchio
:
axis
(
x
,
y
,
z
)
{
axis
.
normalize
();
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE
(
isUnitary
(
axis
)
,
"Translation axis is not unitary"
);
assert
(
isUnitary
(
axis
)
&&
"Translation axis is not unitary"
);
}
template
<
typename
Vector3Like
>
...
...
@@ -479,7 +479,7 @@ namespace pinocchio
:
axis
(
axis
)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
Vector3Like
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE
(
isUnitary
(
axis
)
,
"Translation axis is not unitary"
);
assert
(
isUnitary
(
axis
)
&&
"Translation axis is not unitary"
);
}
JointDataDerived
createData
()
const
{
return
JointDataDerived
(
axis
);
}
...
...
src/multibody/joint/joint-prismatic.hpp
View file @
4a28d1e7
...
...
@@ -291,7 +291,7 @@ namespace pinocchio
JointMotion
__mult__
(
const
Eigen
::
MatrixBase
<
Vector1Like
>
&
v
)
const
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
(
Vector1Like
,
1
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
v
.
size
()
==
1
);
assert
(
v
.
size
()
==
1
);
return
JointMotion
(
v
[
0
]);
}
...
...
@@ -334,7 +334,7 @@ namespace pinocchio
typename
ConstraintForceSetOp
<
ConstraintPrismaticTpl
,
Derived
>::
ReturnType
operator
*
(
const
Eigen
::
MatrixBase
<
Derived
>
&
F
)
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
F
.
rows
()
==
6
);
assert
(
F
.
rows
()
==
6
);
return
F
.
row
(
LINEAR
+
axis
);
}
...
...
src/multibody/joint/joint-revolute-unaligned.hpp
View file @
4a28d1e7
...
...
@@ -494,7 +494,7 @@ namespace pinocchio
:
axis
(
x
,
y
,
z
)
{
axis
.
normalize
();
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE
(
isUnitary
(
axis
)
,
"Rotation axis is not unitary"
);
assert
(
isUnitary
(
axis
)
&&
"Rotation axis is not unitary"
);
}
template
<
typename
Vector3Like
>
...
...
@@ -502,7 +502,7 @@ namespace pinocchio
:
axis
(
axis
)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
Vector3Like
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE
(
isUnitary
(
axis
)
,
"Rotation axis is not unitary"
);
assert
(
isUnitary
(
axis
)
&&
"Rotation axis is not unitary"
);
}
JointDataDerived
createData
()
const
{
return
JointDataDerived
(
axis
);
}
...
...
src/multibody/joint/joint-revolute-unbounded-unaligned.hpp
View file @
4a28d1e7
...
...
@@ -113,7 +113,7 @@ namespace pinocchio
:
axis
(
x
,
y
,
z
)
{
axis
.
normalize
();
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE
(
isUnitary
(
axis
)
,
"Rotation axis is not unitary"
);
assert
(
isUnitary
(
axis
)
&&
"Rotation axis is not unitary"
);
}
template
<
typename
Vector3Like
>
...
...
@@ -121,7 +121,7 @@ namespace pinocchio
:
axis
(
axis
)
{
EIGEN_STATIC_ASSERT_VECTOR_ONLY
(
Vector3Like
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE
(
isUnitary
(
axis
)
,
"Rotation axis is not unitary"
);
assert
(
isUnitary
(
axis
)
&&
"Rotation axis is not unitary"
);
}
JointDataDerived
createData
()
const
{
return
JointDataDerived
(
axis
);
}
...
...
src/multibody/joint/joint-revolute.hpp
View file @
4a28d1e7
...
...
@@ -134,7 +134,7 @@ namespace pinocchio
}
default:
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE
(
false
,
"must nerver happened"
);
assert
(
false
&&
"must nerver happened"
);
break
;
}
}
...
...
@@ -183,7 +183,7 @@ namespace pinocchio
}
default:
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE
(
false
,
"must nerver happened"
);
assert
(
false
&&
"must nerver happened"
);
break
;
}
}
...
...
@@ -402,7 +402,7 @@ namespace pinocchio
typename
ConstraintForceSetOp
<
ConstraintRevoluteTpl
,
Derived
>::
ReturnType
operator
*
(
const
Eigen
::
MatrixBase
<
Derived
>
&
F
)
const
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
F
.
rows
()
==
6
);
assert
(
F
.
rows
()
==
6
);
return
F
.
row
(
ANGULAR
+
axis
);
}
};
// struct TransposeConst
...
...
src/multibody/joint/joint-spherical.hpp
View file @
4a28d1e7
...
...
@@ -225,7 +225,7 @@ namespace pinocchio
const
typename
SizeDepType
<
3
>::
RowsReturn
<
MatrixDerived
>::
ConstType
operator
*
(
const
Eigen
::
MatrixBase
<
MatrixDerived
>
&
F
)
const
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
F
.
rows
()
==
6
);
assert
(
F
.
rows
()
==
6
);
return
F
.
derived
().
template
middleRows
<
3
>(
Inertia
::
ANGULAR
);
}
};
...
...
@@ -410,8 +410,8 @@ namespace pinocchio
typedef
Eigen
::
Map
<
const
Quaternion
>
ConstQuaternionMap
;
ConstQuaternionMap
quat
(
q_joint
.
derived
().
data
());
//
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
math
::
fabs
(
quat
.
coeffs
().
squaredNorm
()
-
1.
)
<=
1e-4
);
//
assert
(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
assert
(
math
::
fabs
(
quat
.
coeffs
().
squaredNorm
()
-
1.
)
<=
1e-4
);
M
.
rotation
(
quat
.
matrix
());
M
.
translation
().
setZero
();
...
...
src/multibody/joint/joint-translation.hpp
View file @
4a28d1e7
...
...
@@ -298,7 +298,7 @@ namespace pinocchio
const
typename
SizeDepType
<
3
>::
RowsReturn
<
MatrixDerived
>::
ConstType
operator
*
(
const
Eigen
::
MatrixBase
<
MatrixDerived
>
&
F
)
const
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
F
.
rows
()
==
6
);
assert
(
F
.
rows
()
==
6
);
return
F
.
derived
().
template
middleRows
<
3
>(
LINEAR
);
}
...
...
src/multibody/liegroup/cartesian-product-variant.hpp
View file @
4a28d1e7
...
...
@@ -94,9 +94,9 @@ namespace pinocchio
const
Eigen
::
MatrixBase
<
Velocity_t
>
&
v
,
const
Eigen
::
MatrixBase
<
ConfigOut_t
>
&
qout
)
const
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
q
.
size
()
==
m_nq
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
v
.
size
()
==
m_nv
);
PINOCCHIO_ASSERT_THROW_AT_RUNTIME
(
qout
.
size
()
==
m_nq
);
assert
(
q
.
size
()
==
m_nq
);
assert
(
v
.
size
()
==
m_nv
);
assert
(
qout
.
size
()
==
m_nq
);
ConfigOut_t
&
qout_
=
PINOCCHIO_EIGEN_CONST_CAST
(
ConfigOut_t
,
qout
);
Index
id_q
=
0
,
id_v
=
0
;
...
...
src/multibody/liegroup/cartesian-product.hpp
View file @
4a28d1e7
...
...
@@ -111,7 +111,7 @@ namespace pinocchio
void
integrateCoeffWiseJacobian_impl
(
const
Eigen
::
MatrixBase
<
Config_t
>
&
q
,
const
Eigen
::
MatrixBase
<
Jacobian_t
>
&
J
)
const
{
PINOCCHIO_ASSERT_THROW_AT_RUNTIME_WITH_MESSAGE
(
J
.
rows
()
==
nq
()
&&
J
.
cols
()
==
nv
()
,
"J is not of the right dimension"
);
assert
(
J
.
rows
()
==
nq
()
&&
J
.
cols
()
==
nv
()
&&
"J is not of the right dimension"
);
Jacobian_t
&
J_
=
PINOCCHIO_EIGEN_CONST_CAST
(
Jacobian_t
,
J
);
J_
.
topRightCorner
(
lg1_
.
nq
(),
lg2_
.
nv
()).
setZero
();
J_
.
bottomLeftCorner
(
lg2_
.
nq
(),
lg1_
.
nv
()).
setZero
();
...
...
Prev
1
2
Next
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