WorldSim - 1.2.0

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
phyhinge.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 "phyhinge.h"
21 #include "private/phyjointprivate.h"
22 #include "private/phyobjectprivate.h"
23 #include "private/worldprivate.h"
24 
25 // This is needed because the isnan and isinf functions are not present windows
26 #ifdef FARSA_WIN
27  #include <float.h>
28  #define isnan(x) _isnan(x)
29 #endif
30 
31 namespace farsa {
32 
33 void PhyHinge::construct( const wVector& axis, const wVector& centre, real startAngle, PhyObject* parent, PhyObject* child, bool cp ) {
34  dof = new PhyDOF( this, axis, centre, false );
35  dofsv << dof;
36  dofv = 1;
37  //--- calculate the localMatrixParent
38  //--- start creating an orthonormal coordinate frame using grammSchmidt procedure
39  localMatrixParent = wMatrix::grammSchmidt(axis);
40  //--- calculate the axis that connect the centre of joint with centre of child object
41  wVector start = centre;
42  wVector end = child->matrix().w_pos;
43  if ( parent ) {
44  start -= parent->matrix().w_pos;
45  }
46  wVector newx = (start-end).normalize();
47  if ( isnan(newx[0]) || isnan(newx[1]) || isnan(newx[2]) ) {
48  // this happens when it is not possible to calculate the axis above,
49  // --- in this case nothing has to be done
50  } else {
51  //--- rotate the matrix in order to align X axis calculated from grammSchmidt procedure
52  // to the axis connecting centres. In this way, angle zero correspond to position at which
53  // child object was connected by this joint
54  real sinAngle = (newx * localMatrixParent.x_ax) % localMatrixParent.z_ax;
55  real cosAngle = newx % localMatrixParent.x_ax;
56  real angle = atan2( sinAngle, cosAngle );
57  wQuaternion ql( axis, angle );
58  localMatrixParent = localMatrixParent * wMatrix( ql, wVector(0,0,0) );
59  }
60  localMatrixParent.w_pos = centre;
61  localMatrixParent.sanitifize();
62 
63  //--- calculate the local matrix respect to child object
64  if ( parent ) {
65  globalMatrixParent = localMatrixParent * parent->matrix();
66  } else {
67  globalMatrixParent = localMatrixParent;
68  }
69  localMatrixChild = globalMatrixParent * child->matrix().inverse();
70 
71  localMatrixParent.w_pos = wVector(0,0,0);
72  //--- it will rotate
73  // the localMatrixParent in order to align the axis
74  // with the requested startAngle
75  wQuaternion q( axis, startAngle );
76  localMatrixParent = localMatrixParent * wMatrix( q, wVector(0,0,0) );
77  localMatrixParent.w_pos = centre;
78  if ( parent ) {
79  globalMatrixParent = localMatrixParent * parent->matrix();
80  } else {
81  globalMatrixParent = localMatrixParent;
82  }
83 
84  dof->setAxis( globalMatrixParent.z_ax );
85  dof->setXAxis( globalMatrixParent.x_ax );
86  dof->setYAxis( globalMatrixParent.y_ax );
87  dof->setCentre( globalMatrixParent.w_pos );
88 
89  if ( !localMatrixParent.sanityCheck() ) {
90  qWarning( "Sanity Check Failed on localMatrixParent" );
91  }
92  if ( !localMatrixChild.sanityCheck() ) {
93  qWarning( "Sanity Check Failed on localMatrixChild" );
94  }
95 
96  if ( cp ) {
97  createPrivateJoint();
98  }
99 }
100 
101 PhyHinge::PhyHinge( const wVector& axis, const wVector& centre, real startAngle, PhyObject* parent, PhyObject* child, bool cp )
102  : PhyJoint( parent, child ) {
103  construct( axis, centre, startAngle, parent, child, cp );
104 }
105 
106 PhyHinge::PhyHinge( const wVector& axis, const wVector& centre, PhyObject* parent, PhyObject* child, bool cp )
107  : PhyJoint( parent, child ) {
108  construct( axis, centre, 0, parent, child, cp );
109 }
110 
111 PhyHinge::PhyHinge( const wVector& axis, real startAngle, PhyObject* parent, PhyObject* child, bool cp )
112  : PhyJoint( parent, child ) {
113  construct( axis, wVector(0,0,0), startAngle, parent, child, cp );
114 }
115 
116 PhyHinge::PhyHinge( const wVector& axis, PhyObject* parent, PhyObject* child, bool cp )
117  : PhyJoint( parent, child ) {
118  construct( axis, wVector(0,0,0), 0, parent, child, cp );
119 }
120 
121 PhyHinge::PhyHinge( const wVector& axis, const wVector& centre, const wVector& x_axis, PhyObject* parent, PhyObject* child, bool cp )
122  : PhyJoint( parent, child ) {
123  dof = new PhyDOF( this, axis, centre, false );
124  dofsv << dof;
125  dofv = 1;
126  //--- calculate the localMatrixParent
127  //--- supposing that axis and x_axis are ortogonals
128  localMatrixParent.x_ax = x_axis;
129  localMatrixParent.y_ax = axis * x_axis;
130  localMatrixParent.z_ax = axis;
131  localMatrixParent.w_pos = centre;
132  localMatrixParent.sanitifize();
133 
134  //--- calculate the local matrix respect to child object
135  if ( parent ) {
136  globalMatrixParent = localMatrixParent * parent->matrix();
137  } else {
138  globalMatrixParent = localMatrixParent;
139  }
140  localMatrixChild = globalMatrixParent * child->matrix().inverse();
141  if ( parent ) {
142  globalMatrixParent = localMatrixParent * parent->matrix();
143  } else {
144  globalMatrixParent = localMatrixParent;
145  }
146 
147  dof->setAxis( globalMatrixParent.z_ax );
148  dof->setXAxis( globalMatrixParent.x_ax );
149  dof->setYAxis( globalMatrixParent.y_ax );
150  dof->setCentre( globalMatrixParent.w_pos );
151 
152  if ( !localMatrixParent.sanityCheck() ) {
153  qWarning( "Sanity Check Failed on localMatrixParent" );
154  }
155  if ( !localMatrixChild.sanityCheck() ) {
156  qWarning( "Sanity Check Failed on localMatrixChild" );
157  }
158 
159  if ( cp ) {
160  createPrivateJoint();
161  }
162 }
163 
164 wVector PhyHinge::centre() const {
165  //--- calculate global matrix if necessary
166  if ( parent() ) {
167  globalMatrixParent = localMatrixParent * parent()->matrix();
168  }
169  return globalMatrixParent.w_pos;
170 }
171 
172 void PhyHinge::createPrivateJoint() {
173 #ifdef WORLDSIM_USE_NEWTON
174  priv->joint = NewtonConstraintCreateUserJoint( worldpriv->world, 6, PhyJointPrivate::userBilateralHandler, 0, priv->child, priv->parent );
175  NewtonJointSetUserData( priv->joint, this );
176 #endif
177 }
178 
179 void PhyHinge::updateJointInfo() {
180  //--- calculate the global matrices
181  //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
182  //--- there is no need to re-assign it because it was already done in constructor
183  if ( parent() ) {
184  globalMatrixParent = localMatrixParent * parent()->matrix();
185  }
186  globalMatrixChild = localMatrixChild * child()->matrix();
187 
188  if ( !globalMatrixParent.sanityCheck() ) {
189  qWarning( "Sanity Check Failed on globalMatrixParent" );
190  }
191  if ( !globalMatrixChild.sanityCheck() ) {
192  qWarning( "Sanity Check Failed on globalMatrixChild" );
193  }
194 
195  dof->setAxis( globalMatrixParent.z_ax );
196  dof->setXAxis( globalMatrixParent.x_ax );
197  dof->setYAxis( globalMatrixParent.y_ax );
198  dof->setCentre( globalMatrixParent.w_pos );
199 
200  PhyDOF* dof = dofsv[0];
201  //--- calculate the rotation assuming the local X axis of joint frame as 0
202  //--- and following the right-hand convention
203  real sinAngle = (globalMatrixParent.x_ax * globalMatrixChild.x_ax) % globalMatrixChild.z_ax;
204  real cosAngle = globalMatrixChild.x_ax % globalMatrixParent.x_ax;
205  real angle = atan2( sinAngle, cosAngle );
206  real vel;
207  // Velocity is computed in two different ways depending on whether the joint is enabled or not:
208  // if it is, we use Newton functions to get the angular velocities (for a better result), otherwise
209  // we revert to computing the difference with the previous position divided by the timestep
210  if (enabled) {
211  //--- the velocity is calculated projecting the angular velocity of objects on the main axis (z_ax)
212  // This code is not general, becuase it is not appliable in the case of parent==NULL
213  // and also return different results respect to the kinematic way
214  // Then [Gianluca] comment it and replaced with the same code used in the kinematic mode
215  wVector omegaParent, omegaChild;
216 #ifdef WORLDSIM_USE_NEWTON
217  if ( parent() ) {
218  NewtonBodyGetOmega( priv->parent, &omegaParent[0] );
219  } else {
220  omegaParent = wVector(0, 0, 0);
221  }
222  NewtonBodyGetOmega( priv->child, &omegaChild[0] );
223 #endif
224  real velP = omegaParent % globalMatrixChild.z_ax;
225  real velC = omegaChild % globalMatrixChild.z_ax;
226  vel = velC - velP;
227  //vel = (angle - dof->position()) / (world()->timeStep());
228  } else {
229  vel = (angle - dof->position()) / (world()->timeStep());
230  }
231  dof->setPosition( angle );
232  dof->setVelocity( vel );
233 }
234 
235 void PhyHinge::updateJoint( real timestep ) {
236  // If the joint is disabled, we don't do anything here
237  if (!enabled) {
238  return;
239  }
240 
241  // Updating the global matrices and the dof
242  updateJointInfo();
243 
244  const real angle = dof->position();
245  const real vel = dof->velocity();
246 
247 #ifdef WORLDSIM_USE_NEWTON
248  //--- Restrict the movement on the centre of the joint along all tree orthonormal direction
249  NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixChild.x_ax[0] );
250  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
251  NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixChild.y_ax[0] );
252  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
253  NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixChild.z_ax[0] );
254  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
255 
256  //--- In order to constraint the rotation about X and Y axis of the joint
257  //--- we use LinearRow (that are stronger) getting a point far from objects along
258  //--- the Z axis. Doing this if the two object rotates about X or Y axes then
259  //--- the difference between qChild and qParent augments and then Newton Engine will apply
260  //--- a corresponding force (that applyied outside the centre of the object will become
261  //--- a torque) that will blocks any rotation about X and Y axis
262  real len = 50000.0;
263  wVector qChild( globalMatrixChild.w_pos + globalMatrixChild.z_ax.scale(len) );
264  wVector qParent( globalMatrixParent.w_pos + globalMatrixParent.z_ax.scale(len) );
265  NewtonUserJointAddLinearRow( priv->joint, &qChild[0], &qParent[0], &globalMatrixChild.x_ax[0] );
266  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
267  NewtonUserJointAddLinearRow( priv->joint, &qChild[0], &qParent[0], &globalMatrixChild.y_ax[0] );
268  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
269 
270  if ( dof->isLimited() ) {
271  real lolimit;
272  real hilimit;
273  dof->limits( lolimit, hilimit );
274  if ( angle < lolimit ) {
275  real relAngle = lolimit - angle; // - lolimit;
276  //--- the command is opposite to the movement required to go away from the limit
277  // so, it will be overrided for going away from the limit
278  NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.z_ax[0] );
279  NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
280  NewtonUserJointSetRowSpringDamperAcceleration( priv->joint, 50000.0, 100 );
281  //--- this allow the joint to move back freely
282  NewtonUserJointSetRowMinimumFriction( priv->joint, 0.0 );
283  return;
284  } else if ( angle > hilimit ) {
285  real relAngle = hilimit - angle; // - hilimit;
286  //--- the command is opposite to the movement required to go away from the limit
287  // so, it will be overrided for going away from the limit
288  NewtonUserJointAddAngularRow( priv->joint, relAngle, &globalMatrixChild.z_ax[0] );
289  NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
290  NewtonUserJointSetRowSpringDamperAcceleration( priv->joint, 50000.0, 100 );
291  //--- this allow the joint to move forth freely
292  NewtonUserJointSetRowMaximumFriction( priv->joint, 0.0 );
293  return;
294  }
295  }
296 
297  //--- if it reach this point means that the joint if far from limits
298  //--- and we check for type of motion and we'll apply the corresponding entity
299  real force, accel, wishangle, wishvel, sign;
300  switch( dof->motion() ) {
301  case PhyDOF::force:
302  //--- force will be used to set-up max and min torque newton will use to
303  //--- resolve the constraint and accel to the double of appliedForce
304  //--- this means that Newton will apply exactly forse amount of torque around
305  //--- the axis
306  force = fabs( dof->appliedForce() );
307  accel = 2.0*dof->appliedForce();
308  NewtonUserJointAddAngularRow( priv->joint, 0.0, &globalMatrixChild.z_ax[0] );
309  NewtonUserJointSetRowMinimumFriction( priv->joint, -force );
310  NewtonUserJointSetRowMaximumFriction( priv->joint, +force );
311  NewtonUserJointSetRowAcceleration( priv->joint, accel );
312  NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
313  break;
314  case PhyDOF::vel:
315  //--- I'm not sure that this implementation is correct
316  accel = 0.8*( dof->desiredVelocity() - vel ) / ( timestep );
317  wishangle = (dof->desiredVelocity()*timestep);
318  NewtonUserJointAddAngularRow( priv->joint, wishangle, &globalMatrixChild.z_ax[0] );
319  NewtonUserJointSetRowAcceleration( priv->joint, accel );
320  NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
321  break;
322  case PhyDOF::off:
323  case PhyDOF::pos:
324  wishangle = dof->desiredPosition() - angle;
325  wishvel = wishangle / timestep;
326  if ( fabs(wishvel) > dof->maxVelocity() ) {
327  sign = (wishvel<0) ? -1.0 : +1.0;
328  wishvel = sign*dof->maxVelocity();
329  }
330  accel = 0.8*( wishvel - vel ) / ( timestep );
331  NewtonUserJointAddAngularRow( priv->joint, wishangle, &globalMatrixChild.z_ax[0] );
332  NewtonUserJointSetRowAcceleration( priv->joint, accel );
333  NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
334  //NewtonUserJointAddAngularRow( priv->joint, wishangle, &globalMatrixChild.z_ax[0] );
335  //NewtonUserJointSetRowSpringDamperAcceleration( priv->joint, 30000.0, 2000 );
336  //NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
337  break;
338  default:
339  break;
340  }
341 #endif
342 
343 }
344 
345 } // end namespace farsa