WorldSim - 1.2.0

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
physuspension.cpp
1 /********************************************************************************
2  * WorldSim -- library for robot simulations *
3  * Copyright (C) 2008-2013 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 "physuspension.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 PhySuspension::PhySuspension( const wVector& axis, const wVector& centre, const wVector& x_axis, PhyObject* parent, PhyObject* child, bool cp )
34  : PhyJoint( parent, child ) {
35  dof = new PhyDOF( this, axis, centre, false );
36  dofsv << dof;
37  dofv = 1;
38  //--- calculate the localMatrixParent
39  //--- supposing that axis and x_axis are ortogonals
40  localMatrixParent.x_ax = x_axis;
41  localMatrixParent.y_ax = axis * x_axis;
42  localMatrixParent.z_ax = axis;
43  localMatrixParent.w_pos = centre;
44  localMatrixParent.sanitifize();
45 
46  //--- calculate the local matrix respect to child object
47  if ( parent ) {
48  globalMatrixParent = localMatrixParent * parent->matrix();
49  } else {
50  globalMatrixParent = localMatrixParent;
51  }
52  localMatrixChild = globalMatrixParent * child->matrix().inverse();
53  if ( parent ) {
54  globalMatrixParent = localMatrixParent * parent->matrix();
55  } else {
56  globalMatrixParent = localMatrixParent;
57  }
58 
59  dof->setAxis( globalMatrixParent.z_ax );
60  dof->setXAxis( globalMatrixParent.x_ax );
61  dof->setYAxis( globalMatrixParent.y_ax );
62  dof->setCentre( globalMatrixParent.w_pos );
63 
64  if ( !localMatrixParent.sanityCheck() ) {
65  qWarning( "Sanity Check Failed on localMatrixParent" );
66  }
67  if ( !localMatrixChild.sanityCheck() ) {
68  qWarning( "Sanity Check Failed on localMatrixChild" );
69  }
70 
71  if ( cp ) {
72  createPrivateJoint();
73  }
74 }
75 
76 wVector PhySuspension::centre() const {
77  //--- calculate global matrix if necessary
78  if ( parent() ) {
79  globalMatrixParent = localMatrixParent * parent()->matrix();
80  }
81  return globalMatrixParent.w_pos;
82 }
83 
84 void PhySuspension::createPrivateJoint() {
85 #ifdef WORLDSIM_USE_NEWTON
86  priv->joint = NewtonConstraintCreateUserJoint( worldpriv->world, 6, PhyJointPrivate::userBilateralHandler, 0, priv->child, priv->parent );
87  NewtonJointSetUserData( priv->joint, this );
88 #endif
89 }
90 
91 void PhySuspension::updateJointInfo() {
92  //--- calculate the global matrices
93  //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
94  //--- there is no need to re-assign it because it was already done in constructor
95  if ( parent() ) {
96  globalMatrixParent = localMatrixParent * parent()->matrix();
97  }
98  globalMatrixChild = localMatrixChild * child()->matrix();
99 
100  if ( !globalMatrixParent.sanityCheck() ) {
101  qWarning( "Sanity Check Failed on globalMatrixParent" );
102  }
103  if ( !globalMatrixChild.sanityCheck() ) {
104  qWarning( "Sanity Check Failed on globalMatrixChild" );
105  }
106 
107  dof->setAxis( globalMatrixParent.z_ax );
108  dof->setXAxis( globalMatrixParent.x_ax );
109  dof->setYAxis( globalMatrixParent.y_ax );
110  dof->setCentre( globalMatrixParent.w_pos );
111 
112  PhyDOF* dof = dofsv[0];
113  //--- calculate the rotation assuming the local X axis of joint frame as 0
114  //--- and following the right-hand convention
115  real sinAngle = (globalMatrixParent.x_ax * globalMatrixChild.x_ax) % globalMatrixChild.z_ax;
116  real cosAngle = globalMatrixChild.x_ax % globalMatrixParent.x_ax;
117  real angle = atan2( sinAngle, cosAngle );
118  real vel;
119  // Velocity is computed in two different ways depending on whether the joint is enabled or not:
120  // if it is, we use Newton functions to get the angular velocities (for a better result), otherwise
121  // we revert to computing the difference with the previous position divided by the timestep
122  if (enabled) {
123  //--- the velocity is calculated projecting the angular velocity of objects on the main axis (z_ax)
124  // This code is not general, becuase it is not appliable in the case of parent==NULL
125  // and also return different results respect to the kinematic way
126  // Then [Gianluca] comment it and replaced with the same code used in the kinematic mode
127  wVector omegaParent, omegaChild;
128 #ifdef WORLDSIM_USE_NEWTON
129  if ( parent() ) {
130  NewtonBodyGetOmega( priv->parent, &omegaParent[0] );
131  } else {
132  omegaParent = wVector(0, 0, 0);
133  }
134  NewtonBodyGetOmega( priv->child, &omegaChild[0] );
135 #endif
136  real velP = omegaParent % globalMatrixChild.z_ax;
137  real velC = omegaChild % globalMatrixChild.z_ax;
138  vel = velC - velP;
139  //vel = (angle - dof->position()) / (world()->timeStep());
140  } else {
141  vel = (angle - dof->position()) / (world()->timeStep());
142  }
143  dof->setPosition( angle );
144  dof->setVelocity( vel );
145 }
146 
147 void PhySuspension::updateJoint( real timestep ) {
148  // If the joint is disabled, we don't do anything here
149  if (!enabled) {
150  return;
151  }
152 
153  // Updating the global matrices and the dof
154  updateJointInfo();
155 
156  const real angle = dof->position();
157  const real vel = dof->velocity();
158 
159 #ifdef WORLDSIM_USE_NEWTON
160  //--- Restrict the movement on the centre of the joint along all tree orthonormal direction
161  NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.x_ax[0] );
162  NewtonUserJointSetRowStiffness( priv->joint, 0.98f );
163  //NewtonUserJointSetRowSpringDamperAcceleration( priv->joint, 20, 10 );
164  NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.y_ax[0] );
165  NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
166  NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.z_ax[0] );
167  NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
168 
169  //--- In order to constraint the rotation about X and Y axis of the joint
170  //--- we use LinearRow (that are stronger) getting a point far from objects along
171  //--- the Z axis. Doing this if the two object rotates about X or Y axes then
172  //--- the difference between qChild and qParent augments and then Newton Engine will apply
173  //--- a corresponding force (that applyied outside the centre of the object will become
174  //--- a torque) that will blocks any rotation about X and Y axis
175  real len = 5000.0;
176  wVector qChild( globalMatrixChild.w_pos + globalMatrixChild.z_ax.scale(len) );
177  wVector qParent( globalMatrixParent.w_pos + globalMatrixParent.z_ax.scale(len) );
178  NewtonUserJointAddLinearRow( priv->joint, &qChild[0], &qParent[0], &globalMatrixParent.x_ax[0] );
179  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
180  NewtonUserJointAddLinearRow( priv->joint, &qChild[0], &qParent[0], &globalMatrixParent.y_ax[0] );
181  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
182 // NewtonUserJointAddAngularRow( priv->joint, 0.0f, &globalMatrixParent.x_ax[0] );
183 // NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
184 // NewtonUserJointAddAngularRow( priv->joint, 0.0f, &globalMatrixParent.y_ax[0] );
185 // NewtonUserJointSetRowStiffness( priv->joint, 0.98f );
186 
187  //--- The only motion supported is by velocity
188  if ( dof->motion() == PhyDOF::vel ) {
189  //--- I'm not sure that this implementation is correct
190  real accel = 0.8f*( dof->desiredVelocity() - vel ) / ( timestep );
191  NewtonUserJointAddAngularRow( priv->joint, 0.0f, &globalMatrixParent.z_ax[0] );
192  NewtonUserJointSetRowAcceleration( priv->joint, accel );
193  NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
194  }
195 #endif
196 
197 }
198 
199 } // end namespace farsa