WorldSim - 1.2.3

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  forceOnJoint = wVector(0.0, 0.0, 0.0);
35  //--- calculate the localMatrixParent
36  //--- supposing that firstAxis and secondAxis are ortogonals
37  localMatrixParent.x_ax = secondAxis;
38  localMatrixParent.y_ax = firstAxis * secondAxis;
39  localMatrixParent.z_ax = firstAxis;
40  localMatrixParent.w_pos = centre;
41  localMatrixParent.sanitifize();
42 
43  //--- calculate the local matrix respect to child object
44  if ( parent ) {
45  globalMatrixParent = localMatrixParent * parent->matrix();
46  } else {
47  globalMatrixParent = localMatrixParent;
48  }
49  localMatrixChild = globalMatrixParent * child->matrix().inverse();
50  if ( parent ) {
51  globalMatrixParent = localMatrixParent * parent->matrix();
52  } else {
53  globalMatrixParent = localMatrixParent;
54  }
55 
56  firstDof->setAxis( globalMatrixParent.z_ax );
57  firstDof->setXAxis( globalMatrixParent.x_ax );
58  firstDof->setYAxis( globalMatrixParent.y_ax );
59  firstDof->setCentre( globalMatrixParent.w_pos );
60 
61  globalMatrixChild = localMatrixChild * child->matrix();
62  secondDof->setAxis( globalMatrixParent.x_ax );
63  secondDof->setXAxis( globalMatrixParent.z_ax );
64  secondDof->setYAxis( globalMatrixParent.y_ax );
65  secondDof->setCentre( globalMatrixParent.w_pos );
66 
67  if ( !localMatrixParent.sanityCheck() ) {
68  qWarning( "Sanity Check Failed" );
69  }
70  if ( !localMatrixChild.sanityCheck() ) {
71  qWarning( "Sanity Check Failed" );
72  }
73 
74  if ( cp ) {
75  createPrivateJoint();
76  }
77 }
78 
79 wVector PhyUniversal::centre() {
80  //--- calculate global matrix if necessary
81  if ( parent() ) {
82  globalMatrixParent = localMatrixParent * parent()->matrix();
83  }
84  return globalMatrixParent.w_pos;
85 };
86 
87 wVector PhyUniversal::getForceOnJoint() const
88 {
89  return forceOnJoint;
90 }
91 
92 void PhyUniversal::createPrivateJoint() {
93 #ifdef WORLDSIM_USE_NEWTON
94  priv->joint = NewtonConstraintCreateUserJoint( worldpriv->world, 6, PhyJointPrivate::userBilateralHandler, 0, priv->child, priv->parent );
95  NewtonJointSetUserData( priv->joint, this );
96 #endif
97 }
98 
99 void PhyUniversal::updateJointInfo() {
100  //--- calculate the global matrices
101  //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
102  //--- there is no need to re-assign it because it was already done in constructor
103  if ( parent() ) {
104  globalMatrixParent = localMatrixParent * parent()->matrix();
105  }
106  globalMatrixChild = localMatrixChild * child()->matrix();
107 
108  if ( !globalMatrixParent.sanityCheck() ) {
109  qWarning( "Sanity Check Failed" );
110  }
111  if ( !globalMatrixChild.sanityCheck() ) {
112  qWarning( "Sanity Check Failed" );
113  }
114 
115  firstDof->setAxis( globalMatrixParent.z_ax );
116  firstDof->setXAxis( globalMatrixParent.x_ax );
117  firstDof->setYAxis( globalMatrixParent.y_ax );
118  firstDof->setCentre( globalMatrixParent.w_pos );
119 
120  //--- calculate the rotation assuming the local X axis of joint frame as 0
121  //--- and following the right-hand convention
122  real sinAngle = (globalMatrixParent.x_ax * globalMatrixChild.x_ax) % globalMatrixChild.z_ax;
123  real cosAngle = globalMatrixChild.x_ax % globalMatrixParent.x_ax;
124  real angle = atan2( sinAngle, cosAngle );
125  firstDof->setPosition( angle );
126 
127  //--- the secondDOF results rotate of angle respect to its local X axis
128  wMatrix tmp = wMatrix::identity();
129  tmp.x_ax = globalMatrixParent.z_ax;
130  tmp.y_ax = globalMatrixParent.y_ax;
131  tmp.z_ax = globalMatrixParent.x_ax;
132  tmp = tmp * wMatrix( wQuaternion( tmp.x_ax, angle ), wVector(0,0,0) );
133  tmp.sanitifize();
134  secondDof->setAxis( tmp.z_ax );
135  secondDof->setXAxis( tmp.x_ax );
136  secondDof->setYAxis( tmp.y_ax );
137  secondDof->setCentre( globalMatrixParent.w_pos );
138 }
139 
140 void PhyUniversal::updateJoint( real timestep ) {
141  //--- calculate the global matrices
142  //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
143  //--- there is no need to re-assign it because it was already done in constructor
144  if ( parent() ) {
145  globalMatrixParent = localMatrixParent * parent()->matrix();
146  }
147  globalMatrixChild = localMatrixChild * child()->matrix();
148 
149  if ( !globalMatrixParent.sanityCheck() ) {
150  qWarning( "Sanity Check Failed" );
151  }
152  if ( !globalMatrixChild.sanityCheck() ) {
153  qWarning( "Sanity Check Failed" );
154  }
155 
156  firstDof->setAxis( globalMatrixParent.z_ax );
157  firstDof->setXAxis( globalMatrixParent.x_ax );
158  firstDof->setYAxis( globalMatrixParent.y_ax );
159  firstDof->setCentre( globalMatrixParent.w_pos );
160 
161  //--- information about secondDOF will be calculated below
162  //--- because it depends on rotation about firstDOF
163 
164 #ifdef WORLDSIM_USE_NEWTON
165  //--- get the pin fixed to the first body
166  const wVector& dir0 = globalMatrixChild.z_ax;
167  //--- get the pin fixed to the second body
168  const wVector& dir1 = globalMatrixParent.x_ax;
169 
170  //--- construct an orthogonal coordinate system with these two vectors
171  wVector dir2 = ( dir0 * dir1 );
172  dir2.normalize();
173  wVector dir3( dir2 * dir0 );
174  dir3.normalize();
175 
176  const wVector& p0 = globalMatrixChild.w_pos;
177  const wVector& p1 = globalMatrixParent.w_pos;
178 
179  real len = 100.0;
180  wVector q0( p0 + dir3.scale(len) );
181  wVector q1( p1 + dir1.scale(len) );
182 
183  //--- Restrict the movement on the centre of the joint along all tree orthonormal direction
184  NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir0[0] );
185  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
186  NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir1[0] );
187  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
188  NewtonUserJointAddLinearRow( priv->joint, &p0[0], &p1[0], &dir2[0] );
189  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
190  //NewtonUserJointAddAngularRow( priv->joint, 0.0, &dir2[0] );
191  //NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
192  NewtonUserJointAddLinearRow( priv->joint, &q0[0], &q1[0], &dir0[0] );
193  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
194 
195  //--- force will be used to set-up max and min torque newton will use to
196  //--- resolve the constraint and accel to the double of appliedForce
197  //--- this means that Newton will apply exactly forse amount of torque around
198  //--- the axis
199  real force = fabs( firstDof->appliedForce() );
200  real accel = 2.0*firstDof->appliedForce();
201 
202  //--- calculate the rotation assuming the local X axis of joint frame as 0
203  //--- and following the right-hand convention
204  real sinAngle = (globalMatrixParent.x_ax * globalMatrixChild.x_ax) % globalMatrixChild.z_ax;
205  real cosAngle = globalMatrixChild.x_ax % globalMatrixParent.x_ax;
206  real angle = atan2( sinAngle, cosAngle );
207  real vel = (angle - firstDof->position()) / timestep;
208  firstDof->setPosition( angle );
209  firstDof->setVelocity( vel );
210  if ( firstDof->isLimited() ) {
211  real lolimit;
212  real hilimit;
213  firstDof->limits( lolimit, hilimit );
214  if ( angle < lolimit ) {
215  real relAngle = lolimit - angle; // - lolimit;
216  NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.z_ax[0] );
217  NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
218  //--- this allow the joint to move back freely
219  NewtonUserJointSetRowMinimumFriction( priv->joint, 0.0 );
220  } else if ( angle > hilimit ) {
221  real relAngle = hilimit - angle; // - hilimit;
222  NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.z_ax[0] );
223  NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
224  //--- this allow the joint to move forth freely
225  NewtonUserJointSetRowMaximumFriction( priv->joint, 0.0 );
226  } else {
227  NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.z_ax[0] );
228  NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
229  NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
230  NewtonUserJointSetRowAcceleration( priv->joint, accel );
231  NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
232  }
233  } else {
234  NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.z_ax[0] );
235  NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
236  NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
237  NewtonUserJointSetRowAcceleration( priv->joint, accel );
238  NewtonUserJointSetRowStiffness( priv->joint, firstDof->stiffness() );
239  }
240 
241  //--- the secondDOF results rotate of angle respect to its local X axis
242  wMatrix tmp = wMatrix::identity();
243  tmp.x_ax = globalMatrixParent.z_ax;
244  tmp.y_ax = globalMatrixParent.y_ax;
245  tmp.z_ax = globalMatrixParent.x_ax;
246  tmp = tmp * wMatrix( wQuaternion( tmp.x_ax, angle ), wVector(0,0,0) );
247  tmp.sanitifize();
248  secondDof->setAxis( tmp.z_ax );
249  secondDof->setXAxis( tmp.x_ax );
250  secondDof->setYAxis( tmp.y_ax );
251  secondDof->setCentre( globalMatrixParent.w_pos );
252 // secondDof->setAxis( globalMatrixChild.x_ax );
253 // secondDof->setXAxis( globalMatrixParent.z_ax );
254 // secondDof->setYAxis( globalMatrixChild.y_ax );
255 // secondDof->setCentre( globalMatrixParent.w_pos );
256 
257  //--- force will be used to set-up max and min torque newton will use to
258  //--- resolve the constraint and accel to the double of appliedForce
259  //--- this means that Newton will apply exactly forse amount of torque around
260  //--- the axis
261  force = fabs( secondDof->appliedForce() );
262  accel = 2.0*secondDof->appliedForce();
263 
264  //--- calculate the rotation assuming the local X axis of joint frame as 0
265  //--- and following the right-hand convention
266  //----------- Not sure check the second Axis rotation reference
267  sinAngle = (globalMatrixParent.z_ax * globalMatrixChild.z_ax) % globalMatrixChild.x_ax;
268  cosAngle = globalMatrixChild.z_ax % globalMatrixParent.z_ax;
269  angle = atan2( sinAngle, cosAngle );
270  vel = (angle - secondDof->position()) / timestep;
271  secondDof->setPosition( angle );
272  secondDof->setVelocity( vel );
273  if ( secondDof->isLimited() ) {
274  real lolimit;
275  real hilimit;
276  secondDof->limits( lolimit, hilimit );
277  if ( angle < lolimit ) {
278  real relAngle = lolimit - angle; // - lolimit;
279  NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.x_ax[0] );
280  NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
281  //--- this allow the joint to move back freely
282  NewtonUserJointSetRowMinimumFriction( priv->joint, 0.0 );
283  } else if ( angle > hilimit ) {
284  real relAngle = hilimit - angle; // - hilimit;
285  NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.x_ax[0] );
286  NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
287  //--- this allow the joint to move forth freely
288  NewtonUserJointSetRowMaximumFriction( priv->joint, 0.0 );
289  } else {
290  NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.x_ax[0] );
291  NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
292  NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
293  NewtonUserJointSetRowAcceleration( priv->joint, accel );
294  NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
295  }
296  } else {
297  NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.x_ax[0] );
298  NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
299  NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
300  NewtonUserJointSetRowAcceleration( priv->joint, accel );
301  NewtonUserJointSetRowStiffness( priv->joint, secondDof->stiffness() );
302  }
303 
304  //--- Retrive forces applied to the joint by the constraints
305  forceOnJoint.x = NewtonUserJointGetRowForce (priv->joint, 0);
306  forceOnJoint.y = NewtonUserJointGetRowForce (priv->joint, 1);
307  forceOnJoint.z = NewtonUserJointGetRowForce (priv->joint, 2);
308 
309 #endif
310 
311 }
312 
313 } // end namespace farsa