1 /* Siconos is a program dedicated to modeling, simulation and control
2  * of non smooth dynamical systems.
3  *
4  * Copyright 2021 INRIA.
5  *
6  * Licensed under the Apache License, Version 2.0 (the "License");
7  * you may not use this file except in compliance with the License.
8  * You may obtain a copy of the License at
9  *
10  * http://www.apache.org/licenses/LICENSE-2.0
11  *
12  * Unless required by applicable law or agreed to in writing, software
13  * distributed under the License is distributed on an "AS IS" BASIS,
14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15  * See the License for the specific language governing permissions and
16  * limitations under the License.
17 */
18 
19 /*! \file FixedJointR.cpp */
20 
21 #include "FixedJointR.hpp"
22 #include <NewtonEulerDS.hpp>
23 #include <Interaction.hpp>
24 #include <boost/math/quaternion.hpp>
25 #include <BlockVector.hpp>
26 #include <cfloat>
27 #include <iostream>
28 
29 // #define DEBUG_BEGIN_END_ONLY
30 // #define DEBUG_STDOUT
31 // #define DEBUG_MESSAGES
32 #include "siconos_debug.h"
33 
34 /*
35  * This file contains some code generated using sympy.  The following
36  * is the necessary predule:
37  *
38  * from sympy import Symbol
39  * from sympy import ccode
40  * import numpy as np
41  *
42  * G10G20d1 = np.array([0,Symbol('_G10G20d1x'),Symbol('_G10G20d1y'),Symbol('_G10G20d1z')])
43  * cq2q10 = np.array([Symbol('_cq2q101'),Symbol('_cq2q102'),
44  *                    Symbol('_cq2q103'),Symbol('_cq2q104')])
45  * q1 = np.array([Symbol('q10'), Symbol('q11'), Symbol('q12'), Symbol('q13')])
46  * q2 = np.array([Symbol('q20'), Symbol('q21'), Symbol('q22'), Symbol('q23')])
47  * G1 = np.array([0, Symbol('X1'), Symbol('Y1'), Symbol('Z1')])
48  * G2 = np.array([0, Symbol('X2'), Symbol('Y2'), Symbol('Z2')])
49  *
50  * qinv = lambda q: np.array([q[0],-q[1],-q[2],-q[3]])
51  * qmul = lambda a,b: np.array([
52  *          a[0] * b[0] - a[1] * b[1] - a[2] * b[2] - a[3] * b[3],
53  *          a[0] * b[1] + a[1] * b[0] + a[2] * b[3] - a[3] * b[2],
54  *          a[0] * b[2] - a[1] * b[3] + a[2] * b[0] + a[3] * b[1],
55  *          a[0] * b[3] + a[1] * b[2] - a[2] * b[1] + a[3] * b[0]])
56  *
57  * unrot = lambda V,q: qmul(qinv(q), qmul(V, q))
58  * rot = lambda V,q: qmul(q, qmul(V, qinv(q)))
59  */
60 
FixedJointR(SP::NewtonEulerDS d1,SP::NewtonEulerDS d2)61 FixedJointR::FixedJointR(SP::NewtonEulerDS d1, SP::NewtonEulerDS d2)
62   : NewtonEulerJointR()
63 {
64   setBasePositions(d1->q(), d2 ? d2->q() : SP::SiconosVector());
65 }
66 
setBasePositions(SP::SiconosVector q1,SP::SiconosVector q2)67 void FixedJointR::setBasePositions(SP::SiconosVector q1, SP::SiconosVector q2)
68 {
69   ::boost::math::quaternion<double> quat1((*q1)(3), (*q1)(4), (*q1)(5), (*q1)(6));
70   ::boost::math::quaternion<double> quat2(q2 ? (*q2)(3) : 1, q2 ? (*q2)(4) : 0,
71                                           q2 ? (*q2)(5) : 0, q2 ? (*q2)(6) : 0);
72   ::boost::math::quaternion<double> cq2q10(1.0 / quat2 * quat1);
73 
74   _cq2q101 = cq2q10.R_component_1();
75   _cq2q102 = cq2q10.R_component_2();
76   _cq2q103 = cq2q10.R_component_3();
77   _cq2q104 = cq2q10.R_component_4();
78 
79   ::boost::math::quaternion<double>    quatG10G20_abs(
80     0, (q2 ? q2->getValue(0) : 0) - q1->getValue(0),
81     (q2 ? q2->getValue(1) : 0) - q1->getValue(1),
82     (q2 ? q2->getValue(2) : 0) - q1->getValue(2));
83   ::boost::math::quaternion<double>    quatBuff(0, 0, 0, 0);
84 
85   quatBuff = 1.0/quat1 * quatG10G20_abs * quat1;
86   _G10G20d1x = quatBuff.R_component_2();
87   _G10G20d1y = quatBuff.R_component_3();
88   _G10G20d1z = quatBuff.R_component_4();
89 }
90 
computeh(double time,const BlockVector & q0,SiconosVector & y)91 void FixedJointR::computeh(double time, const BlockVector& q0, SiconosVector& y)
92 {
93   double X1 = q0.getValue(0);
94   double Y1 = q0.getValue(1);
95   double Z1 = q0.getValue(2);
96   double q10 = q0.getValue(3);
97   double q11 = q0.getValue(4);
98   double q12 = q0.getValue(5);
99   double q13 = q0.getValue(6);
100 
101   double X2 = 0;
102   double Y2 = 0;
103   double Z2 = 0;
104   double q20 = 1;
105   double q21 = 0;
106   double q22 = 0;
107   double q23 = 0;
108 
109   if(q0.numberOfBlocks()>1)
110   {
111     X2 = q0.getValue(7);
112     Y2 = q0.getValue(8);
113     Z2 = q0.getValue(9);
114     q20 = q0.getValue(10);
115     q21 = q0.getValue(11);
116     q22 = q0.getValue(12);
117     q23 = q0.getValue(13);
118   }
119 
120   /* sympy expression:
121    *
122    * G1G2d1 = unrot(G2-G1, q1) - G10G20d1
123    * q2to1 = qmul(q1,qinv(qmul(q2,cq2q10)))
124    *
125    * H = list(G1G2d1[1:]) + list(q2to1[1:])
126    */
127 
128   y.setValue(0, -_G10G20d1x + q10*(q10*(-X1 + X2) - q12*(-Z1 + Z2) + q13*(-Y1 + Y2))
129              - q11*(-q11*(-X1 + X2) - q12*(-Y1 + Y2) - q13*(-Z1 + Z2))
130              - q12*(q10*(-Z1 + Z2) - q11*(-Y1 + Y2) + q12*(-X1 + X2))
131              + q13*(q10*(-Y1 + Y2) + q11*(-Z1 + Z2) - q13*(-X1 + X2)));
132   y.setValue(1, -_G10G20d1y + q10*(q10*(-Y1 + Y2) + q11*(-Z1 + Z2) - q13*(-X1 + X2))
133              + q11*(q10*(-Z1 + Z2) - q11*(-Y1 + Y2) + q12*(-X1 + X2))
134              - q12*(-q11*(-X1 + X2) - q12*(-Y1 + Y2) - q13*(-Z1 + Z2))
135              - q13*(q10*(-X1 + X2) - q12*(-Z1 + Z2) + q13*(-Y1 + Y2)));
136   y.setValue(2, -_G10G20d1z + q10*(q10*(-Z1 + Z2) - q11*(-Y1 + Y2) + q12*(-X1 + X2))
137              - q11*(q10*(-Y1 + Y2) + q11*(-Z1 + Z2) - q13*(-X1 + X2))
138              + q12*(q10*(-X1 + X2) - q12*(-Z1 + Z2) + q13*(-Y1 + Y2))
139              - q13*(-q11*(-X1 + X2) - q12*(-Y1 + Y2) - q13*(-Z1 + Z2)));
140   y.setValue(3, q10*(-_cq2q101*q21 - _cq2q102*q20 + _cq2q103*q23 - _cq2q104*q22)
141              + q11*(_cq2q101*q20 - _cq2q102*q21 - _cq2q103*q22 - _cq2q104*q23)
142              + q12*(-_cq2q101*q23 + _cq2q102*q22 - _cq2q103*q21 - _cq2q104*q20)
143              - q13*(-_cq2q101*q22 - _cq2q102*q23 - _cq2q103*q20 + _cq2q104*q21));
144   y.setValue(4, q10*(-_cq2q101*q22 - _cq2q102*q23 - _cq2q103*q20 + _cq2q104*q21)
145              - q11*(-_cq2q101*q23 + _cq2q102*q22 - _cq2q103*q21 - _cq2q104*q20)
146              + q12*(_cq2q101*q20 - _cq2q102*q21 - _cq2q103*q22 - _cq2q104*q23)
147              + q13*(-_cq2q101*q21 - _cq2q102*q20 + _cq2q103*q23 - _cq2q104*q22));
148   y.setValue(5, q10*(-_cq2q101*q23 + _cq2q102*q22 - _cq2q103*q21 - _cq2q104*q20)
149              + q11*(-_cq2q101*q22 - _cq2q102*q23 - _cq2q103*q20 + _cq2q104*q21)
150              - q12*(-_cq2q101*q21 - _cq2q102*q20 + _cq2q103*q23 - _cq2q104*q22)
151              + q13*(_cq2q101*q20 - _cq2q102*q21 - _cq2q103*q22 - _cq2q104*q23));
152 }
153 
computeJachq(double time,Interaction & inter,SP::BlockVector q0)154 void FixedJointR::computeJachq(double time, Interaction& inter, SP::BlockVector q0)
155 {
156   _jachq->zero();
157 
158   SP::SiconosVector q1 = (q0->getAllVect())[0];
159   double X1 = q1->getValue(0);
160   double Y1 = q1->getValue(1);
161   double Z1 = q1->getValue(2);
162   double q10 = q1->getValue(3);
163   double q11 = q1->getValue(4);
164   double q12 = q1->getValue(5);
165   double q13 = q1->getValue(6);
166 
167   double X2 = 0;
168   double Y2 = 0;
169   double Z2 = 0;
170   double q20 = 1;
171   double q21 = 0;
172   double q22 = 0;
173   double q23 = 0;
174 
175   if(q0->numberOfBlocks()>1)
176   {
177     SP::SiconosVector q2 = (q0->getAllVect())[1];
178     X2 = q2->getValue(0);
179     Y2 = q2->getValue(1);
180     Z2 = q2->getValue(2);
181     q20 = q2->getValue(3);
182     q21 = q2->getValue(4);
183     q22 = q2->getValue(5);
184     q23 = q2->getValue(6);
185     Jd1d2(X1, Y1, Z1, q10, q11, q12, q13, X2, Y2, Z2, q20, q21, q22, q23);
186   }
187   else
188     Jd1(X1, Y1, Z1, q10, q11, q12, q13);
189 }
190 
Jd1d2(double X1,double Y1,double Z1,double q10,double q11,double q12,double q13,double X2,double Y2,double Z2,double q20,double q21,double q22,double q23)191 void FixedJointR::Jd1d2(double X1, double Y1, double Z1,
192                         double q10, double q11, double q12, double q13,
193                         double X2, double Y2, double Z2,
194                         double q20, double q21, double q22, double q23)
195 {
196   /* sympy expression:
197    *
198    * H = list(G1G2d1[1:]) + list(q2to1[1:])
199    * dq = list(G1[1:])+list(q1)+list(G2[1:])+list(q2)
200    * jachq = [[h.diff(d) for d in dq] for h in H]
201    */
202 
203   _jachq->setValue(0, 0, -pow(q10, 2) - pow(q11, 2) + pow(q12, 2) + pow(q13, 2));
204   _jachq->setValue(0, 1, -2*q10*q13 - 2*q11*q12);
205   _jachq->setValue(0, 2, 2*q10*q12 - 2*q11*q13);
206   _jachq->setValue(0, 3, 2*q10*(-X1 + X2) - 2*q12*(-Z1 + Z2) + 2*q13*(-Y1 + Y2));
207   _jachq->setValue(0, 4, q11*(-X1 + X2) - q11*(X1 - X2) + q12*(-Y1 + Y2)
208                    - q12*(Y1 - Y2) + 2*q13*(-Z1 + Z2));
209   _jachq->setValue(0, 5, -q10*(-Z1 + Z2) + q10*(Z1 - Z2) + q11*(-Y1 + Y2)
210                    - q11*(Y1 - Y2) - 2*q12*(-X1 + X2));
211   _jachq->setValue(0, 6, 2*q10*(-Y1 + Y2) + q11*(-Z1 + Z2) - q11*(Z1 - Z2)
212                    - q13*(-X1 + X2) + q13*(X1 - X2));
213   _jachq->setValue(0, 7, pow(q10, 2) + pow(q11, 2) - pow(q12, 2) - pow(q13, 2));
214   _jachq->setValue(0, 8, 2*q10*q13 + 2*q11*q12);
215   _jachq->setValue(0, 9, -2*q10*q12 + 2*q11*q13);
216   _jachq->setValue(0, 10, 0);
217   _jachq->setValue(0, 11, 0);
218   _jachq->setValue(0, 12, 0);
219   _jachq->setValue(0, 13, 0);
220   _jachq->setValue(1, 0, 2*q10*q13 - 2*q11*q12);
221   _jachq->setValue(1, 1, -pow(q10, 2) + pow(q11, 2) - pow(q12, 2) + pow(q13, 2));
222   _jachq->setValue(1, 2, -2*q10*q11 - 2*q12*q13);
223   _jachq->setValue(1, 3, 2*q10*(-Y1 + Y2) + 2*q11*(-Z1 + Z2) - 2*q13*(-X1 + X2));
224   _jachq->setValue(1, 4, 2*q10*(-Z1 + Z2) - q11*(-Y1 + Y2) + q11*(Y1 - Y2)
225                    + q12*(-X1 + X2) - q12*(X1 - X2));
226   _jachq->setValue(1, 5, 2*q11*(-X1 + X2) + q12*(-Y1 + Y2) - q12*(Y1 - Y2)
227                    + q13*(-Z1 + Z2) - q13*(Z1 - Z2));
228   _jachq->setValue(1, 6, -q10*(-X1 + X2) + q10*(X1 - X2) + q12*(-Z1 + Z2)
229                    - q12*(Z1 - Z2) - 2*q13*(-Y1 + Y2));
230   _jachq->setValue(1, 7, -2*q10*q13 + 2*q11*q12);
231   _jachq->setValue(1, 8, pow(q10, 2) - pow(q11, 2) + pow(q12, 2) - pow(q13, 2));
232   _jachq->setValue(1, 9, 2*q10*q11 + 2*q12*q13);
233   _jachq->setValue(1, 10, 0);
234   _jachq->setValue(1, 11, 0);
235   _jachq->setValue(1, 12, 0);
236   _jachq->setValue(1, 13, 0);
237   _jachq->setValue(2, 0, -2*q10*q12 - 2*q11*q13);
238   _jachq->setValue(2, 1, 2*q10*q11 - 2*q12*q13);
239   _jachq->setValue(2, 2, -pow(q10, 2) + pow(q11, 2) + pow(q12, 2) - pow(q13, 2));
240   _jachq->setValue(2, 3, 2*q10*(-Z1 + Z2) - 2*q11*(-Y1 + Y2) + 2*q12*(-X1 + X2));
241   _jachq->setValue(2, 4, -q10*(-Y1 + Y2) + q10*(Y1 - Y2) - 2*q11*(-Z1 + Z2)
242                    + q13*(-X1 + X2) - q13*(X1 - X2));
243   _jachq->setValue(2, 5, 2*q10*(-X1 + X2) - q12*(-Z1 + Z2) + q12*(Z1 - Z2)
244                    + q13*(-Y1 + Y2) - q13*(Y1 - Y2));
245   _jachq->setValue(2, 6, q11*(-X1 + X2) - q11*(X1 - X2) + 2*q12*(-Y1 + Y2)
246                    + q13*(-Z1 + Z2) - q13*(Z1 - Z2));
247   _jachq->setValue(2, 7, 2*q10*q12 + 2*q11*q13);
248   _jachq->setValue(2, 8, -2*q10*q11 + 2*q12*q13);
249   _jachq->setValue(2, 9, pow(q10, 2) - pow(q11, 2) - pow(q12, 2) + pow(q13, 2));
250   _jachq->setValue(2, 10, 0);
251   _jachq->setValue(2, 11, 0);
252   _jachq->setValue(2, 12, 0);
253   _jachq->setValue(2, 13, 0);
254   _jachq->setValue(3, 0, 0);
255   _jachq->setValue(3, 1, 0);
256   _jachq->setValue(3, 2, 0);
257   _jachq->setValue(3, 3, -_cq2q101*q21 - _cq2q102*q20 + _cq2q103*q23 - _cq2q104*q22);
258   _jachq->setValue(3, 4, _cq2q101*q20 - _cq2q102*q21 - _cq2q103*q22 - _cq2q104*q23);
259   _jachq->setValue(3, 5, -_cq2q101*q23 + _cq2q102*q22 - _cq2q103*q21 - _cq2q104*q20);
260   _jachq->setValue(3, 6, _cq2q101*q22 + _cq2q102*q23 + _cq2q103*q20 - _cq2q104*q21);
261   _jachq->setValue(3, 7, 0);
262   _jachq->setValue(3, 8, 0);
263   _jachq->setValue(3, 9, 0);
264   _jachq->setValue(3, 10, _cq2q101*q11 - _cq2q102*q10 + _cq2q103*q13 - _cq2q104*q12);
265   _jachq->setValue(3, 11, -_cq2q101*q10 - _cq2q102*q11 - _cq2q103*q12 - _cq2q104*q13);
266   _jachq->setValue(3, 12, _cq2q101*q13 + _cq2q102*q12 - _cq2q103*q11 - _cq2q104*q10);
267   _jachq->setValue(3, 13, -_cq2q101*q12 + _cq2q102*q13 + _cq2q103*q10 - _cq2q104*q11);
268   _jachq->setValue(4, 0, 0);
269   _jachq->setValue(4, 1, 0);
270   _jachq->setValue(4, 2, 0);
271   _jachq->setValue(4, 3, -_cq2q101*q22 - _cq2q102*q23 - _cq2q103*q20 + _cq2q104*q21);
272   _jachq->setValue(4, 4, _cq2q101*q23 - _cq2q102*q22 + _cq2q103*q21 + _cq2q104*q20);
273   _jachq->setValue(4, 5, _cq2q101*q20 - _cq2q102*q21 - _cq2q103*q22 - _cq2q104*q23);
274   _jachq->setValue(4, 6, -_cq2q101*q21 - _cq2q102*q20 + _cq2q103*q23 - _cq2q104*q22);
275   _jachq->setValue(4, 7, 0);
276   _jachq->setValue(4, 8, 0);
277   _jachq->setValue(4, 9, 0);
278   _jachq->setValue(4, 10, _cq2q101*q12 - _cq2q102*q13 - _cq2q103*q10 + _cq2q104*q11);
279   _jachq->setValue(4, 11, -_cq2q101*q13 - _cq2q102*q12 + _cq2q103*q11 + _cq2q104*q10);
280   _jachq->setValue(4, 12, -_cq2q101*q10 - _cq2q102*q11 - _cq2q103*q12 - _cq2q104*q13);
281   _jachq->setValue(4, 13, _cq2q101*q11 - _cq2q102*q10 + _cq2q103*q13 - _cq2q104*q12);
282   _jachq->setValue(5, 0, 0);
283   _jachq->setValue(5, 1, 0);
284   _jachq->setValue(5, 2, 0);
285   _jachq->setValue(5, 3, -_cq2q101*q23 + _cq2q102*q22 - _cq2q103*q21 - _cq2q104*q20);
286   _jachq->setValue(5, 4, -_cq2q101*q22 - _cq2q102*q23 - _cq2q103*q20 + _cq2q104*q21);
287   _jachq->setValue(5, 5, _cq2q101*q21 + _cq2q102*q20 - _cq2q103*q23 + _cq2q104*q22);
288   _jachq->setValue(5, 6, _cq2q101*q20 - _cq2q102*q21 - _cq2q103*q22 - _cq2q104*q23);
289   _jachq->setValue(5, 7, 0);
290   _jachq->setValue(5, 8, 0);
291   _jachq->setValue(5, 9, 0);
292   _jachq->setValue(5, 10, _cq2q101*q13 + _cq2q102*q12 - _cq2q103*q11 - _cq2q104*q10);
293   _jachq->setValue(5, 11, _cq2q101*q12 - _cq2q102*q13 - _cq2q103*q10 + _cq2q104*q11);
294   _jachq->setValue(5, 12, -_cq2q101*q11 + _cq2q102*q10 - _cq2q103*q13 + _cq2q104*q12);
295   _jachq->setValue(5, 13, -_cq2q101*q10 - _cq2q102*q11 - _cq2q103*q12 - _cq2q104*q13);
296 }
297 
Jd1(double X1,double Y1,double Z1,double q10,double q11,double q12,double q13)298 void FixedJointR::Jd1(double X1, double Y1, double Z1, double q10, double q11, double q12, double q13)
299 {
300   /* sympy expression:
301    *
302    * q2 = np.array([1,0,0,0])
303    * G2 = np.array([0, 0,0,0])
304    * G1G2d1 = unrot(G2-G1, q1) - G10G20d1
305    * q2to1 = qmul(q1,qinv(qmul(q2,cq2q10)))
306    *
307    * H = list(G1G2d1[1:]) + list(q2to1[1:])
308    * dq = list(G1[1:])+list(q1)
309    * jachq = [[h.diff(d) for d in dq] for h in H]
310    */
311 
312   _jachq->setValue(0, 0, -pow(q10, 2) - pow(q11, 2) + pow(q12, 2) + pow(q13, 2));
313   _jachq->setValue(0, 1, -2*q10*q13 - 2*q11*q12);
314   _jachq->setValue(0, 2, 2*q10*q12 - 2*q11*q13);
315   _jachq->setValue(0, 3, -2*X1*q10 - 2*Y1*q13 + 2*Z1*q12);
316   _jachq->setValue(0, 4, -2*X1*q11 - 2*Y1*q12 - 2*Z1*q13);
317   _jachq->setValue(0, 5, 2*X1*q12 - 2*Y1*q11 + 2*Z1*q10);
318   _jachq->setValue(0, 6, 2*X1*q13 - 2*Y1*q10 - 2*Z1*q11);
319   _jachq->setValue(1, 0, 2*q10*q13 - 2*q11*q12);
320   _jachq->setValue(1, 1, -pow(q10, 2) + pow(q11, 2) - pow(q12, 2) + pow(q13, 2));
321   _jachq->setValue(1, 2, -2*q10*q11 - 2*q12*q13);
322   _jachq->setValue(1, 3, 2*X1*q13 - 2*Y1*q10 - 2*Z1*q11);
323   _jachq->setValue(1, 4, -2*X1*q12 + 2*Y1*q11 - 2*Z1*q10);
324   _jachq->setValue(1, 5, -2*X1*q11 - 2*Y1*q12 - 2*Z1*q13);
325   _jachq->setValue(1, 6, 2*X1*q10 + 2*Y1*q13 - 2*Z1*q12);
326   _jachq->setValue(2, 0, -2*q10*q12 - 2*q11*q13);
327   _jachq->setValue(2, 1, 2*q10*q11 - 2*q12*q13);
328   _jachq->setValue(2, 2, -pow(q10, 2) + pow(q11, 2) + pow(q12, 2) - pow(q13, 2));
329   _jachq->setValue(2, 3, -2*X1*q12 + 2*Y1*q11 - 2*Z1*q10);
330   _jachq->setValue(2, 4, -2*X1*q13 + 2*Y1*q10 + 2*Z1*q11);
331   _jachq->setValue(2, 5, -2*X1*q10 - 2*Y1*q13 + 2*Z1*q12);
332   _jachq->setValue(2, 6, -2*X1*q11 - 2*Y1*q12 - 2*Z1*q13);
333   _jachq->setValue(3, 0, 0);
334   _jachq->setValue(3, 1, 0);
335   _jachq->setValue(3, 2, 0);
336   _jachq->setValue(3, 3, -_cq2q102);
337   _jachq->setValue(3, 4, _cq2q101);
338   _jachq->setValue(3, 5, -_cq2q104);
339   _jachq->setValue(3, 6, _cq2q103);
340   _jachq->setValue(4, 0, 0);
341   _jachq->setValue(4, 1, 0);
342   _jachq->setValue(4, 2, 0);
343   _jachq->setValue(4, 3, -_cq2q103);
344   _jachq->setValue(4, 4, _cq2q104);
345   _jachq->setValue(4, 5, _cq2q101);
346   _jachq->setValue(4, 6, -_cq2q102);
347   _jachq->setValue(5, 0, 0);
348   _jachq->setValue(5, 1, 0);
349   _jachq->setValue(5, 2, 0);
350   _jachq->setValue(5, 3, -_cq2q104);
351   _jachq->setValue(5, 4, -_cq2q103);
352   _jachq->setValue(5, 5, _cq2q102);
353   _jachq->setValue(5, 6, _cq2q101);
354 }
355