WorldSim - 1.4.5

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