WorldSim - 1.2.3

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