WorldSim - 1.2.0

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
phyuniversal.cpp
1 /********************************************************************************
2  * WorldSim -- library for robot simulations *
3  * Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it> *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the Free Software *
17  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
18  ********************************************************************************/
19 
20 #include "phyuniversal.h"
21 #include "private/phyjointprivate.h"
22 #include "private/phyobjectprivate.h"
23 #include "private/worldprivate.h"
24 
25 namespace farsa {
26 
27 PhyUniversal::PhyUniversal( const wVector& firstAxis, const wVector& secondAxis, const wVector& centre, PhyObject* parent, PhyObject* child, bool cp )
28  : PhyJoint( parent, child ) {
29  firstDof = new PhyDOF( this, firstAxis, centre, false );
30  dofsv << firstDof;
31  secondDof = new PhyDOF( this, secondAxis, centre, false );
32  dofsv << secondDof;
33  dofv = 2;
34  //--- calculate the localMatrixParent
35  //--- supposing that firstAxis and secondAxis are ortogonals
36  localMatrixParent.x_ax = secondAxis;
37  localMatrixParent.y_ax = firstAxis * secondAxis;
38  localMatrixParent.z_ax = firstAxis;
39  localMatrixParent.w_pos = centre;
40  localMatrixParent.sanitifize();
41 
42  //--- calculate the local matrix respect to child object
43  if ( parent ) {
44  globalMatrixParent = localMatrixParent * parent->matrix();
45  } else {
46  globalMatrixParent = localMatrixParent;
47  }
48  localMatrixChild = globalMatrixParent * child->matrix().inverse();
49  if ( parent ) {
50  globalMatrixParent = localMatrixParent * parent->matrix();
51  } else {
52  globalMatrixParent = localMatrixParent;
53  }
54 
55  firstDof->setAxis( globalMatrixParent.z_ax );
56  firstDof->setXAxis( globalMatrixParent.x_ax );
57  firstDof->setYAxis( globalMatrixParent.y_ax );
58  firstDof->setCentre( globalMatrixParent.w_pos );
59 
60  globalMatrixChild = localMatrixChild * child->matrix();
61  secondDof->setAxis( globalMatrixParent.x_ax );
62  secondDof->setXAxis( globalMatrixParent.z_ax );
63  secondDof->setYAxis( globalMatrixParent.y_ax );
64  secondDof->setCentre( globalMatrixParent.w_pos );
65 
66  if ( !localMatrixParent.sanityCheck() ) {
67  qWarning( "Sanity Check Failed" );
68  }
69  if ( !localMatrixChild.sanityCheck() ) {
70  qWarning( "Sanity Check Failed" );
71  }
72 
73  if ( cp ) {
74  createPrivateJoint();
75  }
76 }
77 
78 wVector PhyUniversal::centre() {
79  //--- calculate global matrix if necessary
80  if ( parent() ) {
81  globalMatrixParent = localMatrixParent * parent()->matrix();
82  }
83  return globalMatrixParent.w_pos;
84 };
85 
86 void PhyUniversal::createPrivateJoint() {
87 #ifdef WORLDSIM_USE_NEWTON
88  priv->joint = NewtonConstraintCreateUserJoint( worldpriv->world, 6, PhyJointPrivate::userBilateralHandler, 0, priv->child, priv->parent );
89  NewtonJointSetUserData( priv->joint, this );
90 #endif
91 }
92 
93 void PhyUniversal::updateJointInfo() {
94  //--- calculate the global matrices
95  //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
96  //--- there is no need to re-assign it because it was already done in constructor
97  if ( parent() ) {
98  globalMatrixParent = localMatrixParent * parent()->matrix();
99  }
100  globalMatrixChild = localMatrixChild * child()->matrix();
101 
102  if ( !globalMatrixParent.sanityCheck() ) {
103  qWarning( "Sanity Check Failed" );
104  }
105  if ( !globalMatrixChild.sanityCheck() ) {
106  qWarning( "Sanity Check Failed" );
107  }
108 
109  firstDof->setAxis( globalMatrixParent.z_ax );
110  firstDof->setXAxis( globalMatrixParent.x_ax );
111  firstDof->setYAxis( globalMatrixParent.y_ax );
112  firstDof->setCentre( globalMatrixParent.w_pos );
113 
114  //--- calculate the rotation assuming the local X axis of joint frame as 0
115  //--- and following the right-hand convention
116  real sinAngle = (globalMatrixParent.x_ax * globalMatrixChild.x_ax) % globalMatrixChild.z_ax;
117  real cosAngle = globalMatrixChild.x_ax % globalMatrixParent.x_ax;
118  real angle = atan2( sinAngle, cosAngle );
119  firstDof->setPosition( angle );
120 
121  //--- the secondDOF results rotate of angle respect to its local X axis
122  wMatrix tmp = wMatrix::identity();
123  tmp.x_ax = globalMatrixParent.z_ax;
124  tmp.y_ax = globalMatrixParent.y_ax;
125  tmp.z_ax = globalMatrixParent.x_ax;
126  tmp = tmp * wMatrix( wQuaternion( tmp.x_ax, angle ), wVector(0,0,0) );
127  tmp.sanitifize();
128  secondDof->setAxis( tmp.z_ax );
129  secondDof->setXAxis( tmp.x_ax );
130  secondDof->setYAxis( tmp.y_ax );
131  secondDof->setCentre( globalMatrixParent.w_pos );
132 }
133 
134 void PhyUniversal::updateJoint( real timestep ) {
135  //--- calculate the global matrices
136  //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
137  //--- there is no need to re-assign it because it was already done in constructor
138  if ( parent() ) {
139  globalMatrixParent = localMatrixParent * parent()->matrix();
140  }
141  globalMatrixChild = localMatrixChild * child()->matrix();
142 
143  if ( !globalMatrixParent.sanityCheck() ) {
144  qWarning( "Sanity Check Failed" );
145  }
146  if ( !globalMatrixChild.sanityCheck() ) {
147  qWarning( "Sanity Check Failed" );
148  }
149 
150  firstDof->setAxis( globalMatrixParent.z_ax );
151  firstDof->setXAxis( globalMatrixParent.x_ax );
152  firstDof->setYAxis( globalMatrixParent.y_ax );
153  firstDof->setCentre( globalMatrixParent.w_pos );
154 
155  //--- information about secondDOF will be calculated below
156  //--- because it depends on rotation about firstDOF
157 
158 #ifdef WORLDSIM_USE_NEWTON
159  //--- get the pin fixed to the first body
160  const wVector& dir0 = globalMatrixChild.z_ax;
161  //--- get the pin fixed to the second body
162  const wVector& dir1 = globalMatrixParent.x_ax;
163 
164  //--- construct an orthogonal coordinate system with these two vectors
165  wVector dir2 = ( dir0 * dir1 );
166  dir2.normalize();
167  wVector dir3( dir2 * dir0 );
168  dir3.normalize();
169 
170  const wVector& p0 = globalMatrixChild.w_pos;
171  const wVector& p1 = globalMatrixParent.w_pos;
172 
173  real len = 100.0;
174  wVector q0( p0 + dir3.scale(len) );
175  wVector q1( p1 + dir1.scale(len) );
176 
177  //--- Restrict the movement on the centre of the joint along all tree orthonormal direction
178  NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir0[0] );
179  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
180  NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir1[0] );
181  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
182  NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir2[0] );
183  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
184  //NewtonUserJointAddAngularRow( priv->joint, 0.0, &dir2[0] );
185  //NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
186  NewtonUserJointAddLinearRow( priv->joint, &q0[0], &q1[0], &dir0[0] );
187  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
188 
189  //--- force will be used to set-up max and min torque newton will use to
190  //--- resolve the constraint and accel to the double of appliedForce
191  //--- this means that Newton will apply exactly forse amount of torque around
192  //--- the axis
193  real force = fabs( firstDof->appliedForce() );
194  real accel = 2.0*firstDof->appliedForce();
195 
196  //--- calculate the rotation assuming the local X axis of joint frame as 0
197  //--- and following the right-hand convention
198  real sinAngle = (globalMatrixParent.x_ax * globalMatrixChild.x_ax) % globalMatrixChild.z_ax;
199  real cosAngle = globalMatrixChild.x_ax % globalMatrixParent.x_ax;
200  real angle = atan2( sinAngle, cosAngle );
201  real vel = (angle - firstDof->position()) / timestep;
202  firstDof->setPosition( angle );
203  firstDof->setVelocity( vel );
204  if ( firstDof->isLimited() ) {
205  real lolimit;
206  real hilimit;
207  firstDof->limits( lolimit, hilimit );
208  if ( angle < lolimit ) {
209  real relAngle = lolimit - angle; // - lolimit;
210  NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.z_ax[0] );
211  NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
212  //--- this allow the joint to move back freely
213  NewtonUserJointSetRowMinimumFriction( priv->joint, 0.0 );
214  } else if ( angle > hilimit ) {
215  real relAngle = hilimit - angle; // - hilimit;
216  NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.z_ax[0] );
217  NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
218  //--- this allow the joint to move forth freely
219  NewtonUserJointSetRowMaximumFriction( priv->joint, 0.0 );
220  } else {
221  NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.z_ax[0] );
222  NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
223  NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
224  NewtonUserJointSetRowAcceleration( priv->joint, accel );
225  NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
226  }
227  } else {
228  NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.z_ax[0] );
229  NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
230  NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
231  NewtonUserJointSetRowAcceleration( priv->joint, accel );
232  NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
233  }
234 
235  //--- the secondDOF results rotate of angle respect to its local X axis
236  wMatrix tmp = wMatrix::identity();
237  tmp.x_ax = globalMatrixParent.z_ax;
238  tmp.y_ax = globalMatrixParent.y_ax;
239  tmp.z_ax = globalMatrixParent.x_ax;
240  tmp = tmp * wMatrix( wQuaternion( tmp.x_ax, angle ), wVector(0,0,0) );
241  tmp.sanitifize();
242  secondDof->setAxis( tmp.z_ax );
243  secondDof->setXAxis( tmp.x_ax );
244  secondDof->setYAxis( tmp.y_ax );
245  secondDof->setCentre( globalMatrixParent.w_pos );
246 // secondDof->setAxis( globalMatrixChild.x_ax );
247 // secondDof->setXAxis( globalMatrixParent.z_ax );
248 // secondDof->setYAxis( globalMatrixChild.y_ax );
249 // secondDof->setCentre( globalMatrixParent.w_pos );
250 
251  //--- force will be used to set-up max and min torque newton will use to
252  //--- resolve the constraint and accel to the double of appliedForce
253  //--- this means that Newton will apply exactly forse amount of torque around
254  //--- the axis
255  force = fabs( secondDof->appliedForce() );
256  accel = 2.0*secondDof->appliedForce();
257 
258  //--- calculate the rotation assuming the local X axis of joint frame as 0
259  //--- and following the right-hand convention
260  //----------- Not sure check the second Axis rotation reference
261  sinAngle = (globalMatrixParent.z_ax * globalMatrixChild.z_ax) % globalMatrixChild.x_ax;
262  cosAngle = globalMatrixChild.z_ax % globalMatrixParent.z_ax;
263  angle = atan2( sinAngle, cosAngle );
264  vel = (angle - secondDof->position()) / timestep;
265  secondDof->setPosition( angle );
266  secondDof->setVelocity( vel );
267  if ( secondDof->isLimited() ) {
268  real lolimit;
269  real hilimit;
270  secondDof->limits( lolimit, hilimit );
271  if ( angle < lolimit ) {
272  real relAngle = lolimit - angle; // - lolimit;
273  NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.x_ax[0] );
274  NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
275  //--- this allow the joint to move back freely
276  NewtonUserJointSetRowMinimumFriction( priv->joint, 0.0 );
277  } else if ( angle > hilimit ) {
278  real relAngle = hilimit - angle; // - hilimit;
279  NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.x_ax[0] );
280  NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
281  //--- this allow the joint to move forth freely
282  NewtonUserJointSetRowMaximumFriction( priv->joint, 0.0 );
283  } else {
284  NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.x_ax[0] );
285  NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
286  NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
287  NewtonUserJointSetRowAcceleration( priv->joint, accel );
288  NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
289  }
290  } else {
291  NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.x_ax[0] );
292  NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
293  NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
294  NewtonUserJointSetRowAcceleration( priv->joint, accel );
295  NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
296  }
297 
298 #endif
299 
300 }
301 
302 } // end namespace farsa