| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2020 INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_algorithm_frames_derivatives_hxx__ |
||
6 |
#define __pinocchio_algorithm_frames_derivatives_hxx__ |
||
7 |
|||
8 |
#include "pinocchio/multibody/visitor.hpp" |
||
9 |
#include "pinocchio/algorithm/check.hpp" |
||
10 |
#include "pinocchio/algorithm/kinematics-derivatives.hpp" |
||
11 |
|||
12 |
namespace pinocchio |
||
13 |
{ |
||
14 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2> |
||
15 |
void |
||
16 |
6 |
getFrameVelocityDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
17 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
18 |
const FrameIndex frame_id, |
||
19 |
const ReferenceFrame rf, |
||
20 |
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, |
||
21 |
const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv) |
||
22 |
{ |
||
23 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
24 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
25 |
|||
26 |
typedef typename Data::Matrix6x Matrix6x; |
||
27 |
|||
28 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1,Matrix6x); |
||
29 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2,Matrix6x); |
||
30 |
|||
31 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
6 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dq.cols(), model.nv); |
32 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
6 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dv.cols(), model.nv); |
33 |
✓✗✓✗ |
6 |
assert(model.check(data) && "data is not consistent with model."); |
34 |
|||
35 |
✗✓✗✗ |
6 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(frame_id <= model.frames.size(),"frame_id is larger than the number of frames"); |
36 |
typedef typename Model::Frame Frame; |
||
37 |
typedef typename Data::Motion Motion; |
||
38 |
6 |
const Frame & frame = model.frames[frame_id]; |
|
39 |
6 |
const JointIndex joint_id = frame.parent; |
|
40 |
|||
41 |
6 |
Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq); |
|
42 |
6 |
Matrix6xOut2 & v_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv); |
|
43 |
✓✗ | 6 |
getJointVelocityDerivatives(model,data,joint_id,rf, |
44 |
v_partial_dq_,v_partial_dv_); |
||
45 |
|||
46 |
// Update frame placement |
||
47 |
6 |
typename Data::SE3 & oMframe = data.oMf[frame_id]; |
|
48 |
✓✗ | 6 |
oMframe = data.oMi[joint_id] * frame.placement; |
49 |
|||
50 |
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut1>::Type ColsBlockOut1; |
||
51 |
typedef MotionRef<ColsBlockOut1> MotionOut1; |
||
52 |
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut2>::Type ColsBlockOut2; |
||
53 |
typedef MotionRef<ColsBlockOut2> MotionOut2; |
||
54 |
|||
55 |
✓✗ | 6 |
Motion v_tmp; |
56 |
✓✗✓✗ ✓✗✓✗ |
6 |
const typename Data::SE3::Vector3 trans = data.oMi[joint_id].rotation() * frame.placement.translation(); |
57 |
✓✗✓✗ |
6 |
const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1; |
58 |
✓✓✓✗ |
6 |
switch (rf) |
59 |
{ |
||
60 |
2 |
case WORLD: |
|
61 |
// Do nothing |
||
62 |
2 |
break; |
|
63 |
|||
64 |
2 |
case LOCAL_WORLD_ALIGNED: |
|
65 |
✓✓ | 22 |
for(Eigen::DenseIndex col_id=colRef;col_id>=0;col_id=data.parents_fromRow[(size_t)col_id]) |
66 |
{ |
||
67 |
✓✗✓✗ |
20 |
MotionOut1 m1(v_partial_dq_.col(col_id)); |
68 |
✓✗✓✗ ✓✗✓✗ |
20 |
m1.linear() -= trans.cross(m1.angular()); |
69 |
✓✗✓✗ |
20 |
MotionOut2 m2(v_partial_dv_.col(col_id)); |
70 |
✓✗✓✗ ✓✗✓✗ |
20 |
m2.linear() -= trans.cross(m2.angular()); |
71 |
} |
||
72 |
2 |
break; |
|
73 |
|||
74 |
2 |
case LOCAL: |
|
75 |
✓✓ | 22 |
for(Eigen::DenseIndex col_id=colRef;col_id>=0;col_id=data.parents_fromRow[(size_t)col_id]) |
76 |
{ |
||
77 |
✓✗✓✗ |
20 |
v_tmp = v_partial_dq_.col(col_id); |
78 |
✓✗✓✗ ✓✗✓✗ |
20 |
MotionOut1(v_partial_dq_.col(col_id)) = frame.placement.actInv(v_tmp); |
79 |
✓✗✓✗ |
20 |
v_tmp = v_partial_dv_.col(col_id); |
80 |
✓✗✓✗ ✓✗✓✗ |
20 |
MotionOut2(v_partial_dv_.col(col_id)) = frame.placement.actInv(v_tmp); |
81 |
} |
||
82 |
2 |
break; |
|
83 |
|||
84 |
default: |
||
85 |
break; |
||
86 |
} |
||
87 |
|||
88 |
6 |
} |
|
89 |
|||
90 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4> |
||
91 |
void |
||
92 |
9 |
getFrameAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
93 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
94 |
const FrameIndex frame_id, |
||
95 |
const ReferenceFrame rf, |
||
96 |
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, |
||
97 |
const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq, |
||
98 |
const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv, |
||
99 |
const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da) |
||
100 |
{ |
||
101 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
102 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
103 |
|||
104 |
typedef typename Data::Matrix6x Matrix6x; |
||
105 |
|||
106 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1,Matrix6x); |
||
107 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2,Matrix6x); |
||
108 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut3,Matrix6x); |
||
109 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut4,Matrix6x); |
||
110 |
|||
111 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
9 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v_partial_dq.cols(), model.nv); |
112 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
9 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_dq.cols(), model.nv); |
113 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
9 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_dv.cols(), model.nv); |
114 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
9 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(a_partial_da.cols(), model.nv); |
115 |
✓✗✓✗ |
9 |
assert(model.check(data) && "data is not consistent with model."); |
116 |
|||
117 |
✗✓✗✗ |
9 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(frame_id <= model.frames.size(),"frame_id is larger than the number of frames"); |
118 |
typedef typename Model::Frame Frame; |
||
119 |
typedef typename Data::Motion Motion; |
||
120 |
9 |
const Frame & frame = model.frames[frame_id]; |
|
121 |
9 |
const JointIndex joint_id = frame.parent; |
|
122 |
|||
123 |
9 |
Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq); |
|
124 |
9 |
Matrix6xOut2 & a_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,a_partial_dq); |
|
125 |
9 |
Matrix6xOut3 & a_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dv); |
|
126 |
9 |
Matrix6xOut4 & a_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4,a_partial_da); |
|
127 |
|||
128 |
✓✗ | 9 |
getJointAccelerationDerivatives(model,data,joint_id,rf, |
129 |
v_partial_dq_,a_partial_dq_,a_partial_dv_,a_partial_da_); |
||
130 |
|||
131 |
// Update frame placement |
||
132 |
9 |
typename Data::SE3 & oMframe = data.oMf[frame_id]; |
|
133 |
✓✗ | 9 |
oMframe = data.oMi[joint_id] * frame.placement; |
134 |
|||
135 |
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut1>::Type ColsBlockOut1; |
||
136 |
typedef MotionRef<ColsBlockOut1> MotionOut1; |
||
137 |
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut2>::Type ColsBlockOut2; |
||
138 |
typedef MotionRef<ColsBlockOut2> MotionOut2; |
||
139 |
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut3>::Type ColsBlockOut3; |
||
140 |
typedef MotionRef<ColsBlockOut3> MotionOut3; |
||
141 |
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut4>::Type ColsBlockOut4; |
||
142 |
typedef MotionRef<ColsBlockOut4> MotionOut4; |
||
143 |
|||
144 |
✓✗ | 9 |
Motion v_tmp; |
145 |
✓✗✓✗ ✓✗✓✗ |
9 |
const typename Data::SE3::Vector3 trans = data.oMi[joint_id].rotation() * frame.placement.translation(); |
146 |
✓✗✓✗ |
9 |
const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1; |
147 |
✓✓✓✗ |
9 |
switch (rf) |
148 |
{ |
||
149 |
3 |
case WORLD: |
|
150 |
// Do nothing |
||
151 |
3 |
break; |
|
152 |
|||
153 |
3 |
case LOCAL_WORLD_ALIGNED: |
|
154 |
✓✓ | 33 |
for(Eigen::DenseIndex col_id=colRef;col_id>=0;col_id=data.parents_fromRow[(size_t)col_id]) |
155 |
{ |
||
156 |
✓✗✓✗ |
30 |
MotionOut1 m1(v_partial_dq_.col(col_id)); |
157 |
✓✗✓✗ ✓✗✓✗ |
30 |
m1.linear() -= trans.cross(m1.angular()); |
158 |
✓✗✓✗ |
30 |
MotionOut2 m2(a_partial_dq_.col(col_id)); |
159 |
✓✗✓✗ ✓✗✓✗ |
30 |
m2.linear() -= trans.cross(m2.angular()); |
160 |
✓✗✓✗ |
30 |
MotionOut3 m3(a_partial_dv_.col(col_id)); |
161 |
✓✗✓✗ ✓✗✓✗ |
30 |
m3.linear() -= trans.cross(m3.angular()); |
162 |
✓✗✓✗ |
30 |
MotionOut4 m4(a_partial_da_.col(col_id)); |
163 |
✓✗✓✗ ✓✗✓✗ |
30 |
m4.linear() -= trans.cross(m4.angular()); |
164 |
} |
||
165 |
3 |
break; |
|
166 |
|||
167 |
3 |
case LOCAL: |
|
168 |
✓✓ | 33 |
for(Eigen::DenseIndex col_id=colRef;col_id>=0;col_id=data.parents_fromRow[(size_t)col_id]) |
169 |
{ |
||
170 |
✓✗✓✗ |
30 |
v_tmp = v_partial_dq_.col(col_id); |
171 |
✓✗✓✗ ✓✗✓✗ |
30 |
MotionOut1(v_partial_dq_.col(col_id)) = frame.placement.actInv(v_tmp); |
172 |
✓✗✓✗ |
30 |
v_tmp = a_partial_dq_.col(col_id); |
173 |
✓✗✓✗ ✓✗✓✗ |
30 |
MotionOut2(a_partial_dq_.col(col_id)) = frame.placement.actInv(v_tmp); |
174 |
✓✗✓✗ |
30 |
v_tmp = a_partial_dv_.col(col_id); |
175 |
✓✗✓✗ ✓✗✓✗ |
30 |
MotionOut3(a_partial_dv_.col(col_id)) = frame.placement.actInv(v_tmp); |
176 |
✓✗✓✗ |
30 |
v_tmp = a_partial_da_.col(col_id); |
177 |
✓✗✓✗ ✓✗✓✗ |
30 |
MotionOut4(a_partial_da_.col(col_id)) = frame.placement.actInv(v_tmp); |
178 |
} |
||
179 |
3 |
break; |
|
180 |
|||
181 |
default: |
||
182 |
break; |
||
183 |
} |
||
184 |
9 |
} |
|
185 |
|||
186 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5> |
||
187 |
3 |
void getFrameAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
188 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
189 |
const FrameIndex frame_id, |
||
190 |
const ReferenceFrame rf, |
||
191 |
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq, |
||
192 |
const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv, |
||
193 |
const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq, |
||
194 |
const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv, |
||
195 |
const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da) |
||
196 |
{ |
||
197 |
3 |
getFrameAccelerationDerivatives(model,data, |
|
198 |
frame_id,rf, |
||
199 |
3 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq), |
|
200 |
3 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dq), |
|
201 |
3 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4,a_partial_dv), |
|
202 |
3 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5,a_partial_da)); |
|
203 |
|||
204 |
3 |
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv) = a_partial_da; |
|
205 |
3 |
} |
|
206 |
} |
||
207 |
|||
208 |
#endif // ifndef __pinocchio_algorithm_frames_derivatives_hxx__ |
| Generated by: GCOVR (Version 4.2) |