WorldSim - 1.4.5

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
physlider.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 "physlider.h"
21 #include "mathutils.h"
22 #include "private/phyjointprivate.h"
23 #include "private/phyobjectprivate.h"
24 #include "private/worldprivate.h"
25 
26 #if defined(__GNUC__) && defined(DEVELOPER_WARNINGS)
27  #warning THIS SHOULD BE COMPLETED (ESPECIALLY THE CONTROL PART, THIS CAN ONLY BE CONTROLLED BY FORCE) AND THE COMMENTS IMPROVED
28 #endif
29 
30 namespace farsa {
31 
32 PhySlider::PhySlider( const wVector& axis, PhyObject* parent, PhyObject* child, bool cp )
33  : PhyJoint( parent, child ) {
34  dof = new PhyDOF( this, axis, wVector(), false );
35  dofsv << dof;
36  dofv = 1;
37  forceOnJoint = wVector(0.0, 0.0, 0.0);
38 
39  //--- calculate the localMatrixParent using the Gramm Schmidt procedure
40  localMatrixParent = wMatrix::grammSchmidt(axis);
41 
42  //--- calculate the local matrix respect to child object
43  if ( parent ) {
44  globalMatrixParent = localMatrixParent * parent->matrix();
45  } else {
46  globalMatrixParent = localMatrixParent;
47  }
48  localMatrixChild = globalMatrixParent * child->matrix().inverse();
49 
50  dof->setAxis( globalMatrixParent.z_ax );
51  dof->setXAxis( globalMatrixParent.x_ax );
52  dof->setYAxis( globalMatrixParent.y_ax );
53  dof->setCentre( globalMatrixParent.w_pos );
54 
55  if ( !localMatrixParent.sanityCheck() ) {
56  qWarning( "Sanity Check Failed on localMatrixParent" );
57  }
58  if ( !localMatrixChild.sanityCheck() ) {
59  qWarning( "Sanity Check Failed on localMatrixChild" );
60  }
61 
62  if ( cp ) {
63  createPrivateJoint();
64  }
65 }
66 
67 wVector PhySlider::centre() const {
68  //--- calculate global matrix if necessary
69  if ( parent() ) {
70  globalMatrixParent = localMatrixParent * parent()->matrix();
71  }
72  return globalMatrixParent.w_pos;
73 }
74 
75 wVector PhySlider::getForceOnJoint() const
76 {
77  return forceOnJoint;
78 }
79 
80 void PhySlider::createPrivateJoint() {
81 #ifdef WORLDSIM_USE_NEWTON
82  priv->joint = NewtonConstraintCreateUserJoint( worldpriv->world, 6, PhyJointPrivate::userBilateralHandler, 0, priv->child, priv->parent );
83  NewtonJointSetUserData( priv->joint, this );
84 #endif
85 }
86 
87 void PhySlider::updateJointInfo() {
88  //--- calculate the global matrices
89  //--- if parent doesn't exist, then globalMatrixParent and localMatrixParent coincide and
90  //--- there is no need to re-assign it because it was already done in constructor
91  if ( parent() ) {
92  globalMatrixParent = localMatrixParent * parent()->matrix();
93  }
94  globalMatrixChild = localMatrixChild * child()->matrix();
95 
96  if ( !globalMatrixParent.sanityCheck() ) {
97  qWarning( "Sanity Check Failed on globalMatrixParent" );
98  }
99  if ( !globalMatrixChild.sanityCheck() ) {
100  qWarning( "Sanity Check Failed on globalMatrixChild" );
101  }
102 
103  dof->setAxis( globalMatrixParent.z_ax );
104  dof->setXAxis( globalMatrixParent.x_ax );
105  dof->setYAxis( globalMatrixParent.y_ax );
106  dof->setCentre( globalMatrixParent.w_pos );
107 
108  real pos = (globalMatrixChild.w_pos - globalMatrixParent.w_pos) % globalMatrixParent.z_ax;
109 
110  real vel;
111  // Velocity is computed in two different ways depending on whether the joint is enabled or not:
112  // if it is, we use Newton functions to get the angular velocities (for a better result), otherwise
113  // we revert to computing the difference with the previous position divided by the timestep
114  if (enabled) {
115  //--- the velocity is calculated projecting the angular velocity of objects on the main axis (z_ax)
116  // This code is not general, becuase it is not appliable in the case of parent==NULL
117  // and also return different results respect to the kinematic way
118  // Then [Gianluca] comment it and replaced with the same code used in the kinematic mode
119 // wVector omegaParent, omegaChild;
120  wVector velocityParent, velocityChild;
121 #ifdef WORLDSIM_USE_NEWTON
122  if ( parent() ) {
123  NewtonBodyGetVelocity(priv->parent, &velocityParent[0]);
124  } else {
125  velocityParent = wVector(0, 0, 0);
126  }
127  NewtonBodyGetVelocity(priv->child, &velocityChild[0]);
128 #endif
129  real velP = velocityParent % globalMatrixChild.z_ax;
130  real velC = velocityChild % globalMatrixChild.z_ax;
131  vel = velC - velP;
132 // printf("slider: velocity: === %f ===\n", vel);
133  } else {
134  vel = (pos - dof->position()) / (world()->timeStep());
135  }
136 // printf("slider: position: === %f ===\n", pos);
137  dof->setPosition( pos );
138  dof->setVelocity( vel );
139 }
140 
141 void PhySlider::updateJoint( real timestep ) {
142  // If the joint is disabled, we don't do anything here
143  if (!enabled) {
144  return;
145  }
146 
147  // Updating the global matrices and the dof
148  updateJointInfo();
149 
150  const real pos = dof->position();
151  const real vel = dof->velocity();
152 
153 #ifdef WORLDSIM_USE_NEWTON
154 
155  wVector childPos = globalMatrixChild.w_pos;
156  //--- Restrict the movement on the centre of the joint along all tree orthonormal direction
157  NewtonUserJointAddLinearRow( priv->joint, &childPos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.x_ax[0] );
158  NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
159  NewtonUserJointAddLinearRow( priv->joint, &childPos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.y_ax[0] );
160  NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
161 // NewtonUserJointAddLinearRow( priv->joint, &childPos[0], &globalMatrixParent.w_pos[0], &globalMatrixParent.z_ax[0] );
162 // NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
163 
164  //--- In order to constraint the rotation about X and Y axis of the joint
165  //--- we use LinearRow (that are stronger) getting a point far from objects along
166  //--- the Z axis. Doing this if the two object rotates about X or Y axes then
167  //--- the difference between qChild and qParent augments and then Newton Engine will apply
168  //--- a corresponding force (that applyied outside the centre of the object will become
169  //--- a torque) that will blocks any rotation about X and Y axis
170  real len = 5000.0;
171  wVector qChild( globalMatrixChild.w_pos + globalMatrixChild.z_ax.scale(len) );
172  wVector qParent( globalMatrixParent.w_pos + globalMatrixParent.z_ax.scale(len) );
173  NewtonUserJointAddLinearRow( priv->joint, &qChild[0], &qParent[0], &globalMatrixParent.x_ax[0] );
174  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
175  NewtonUserJointAddLinearRow( priv->joint, &qChild[0], &qParent[0], &globalMatrixParent.y_ax[0] );
176  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
177 // NewtonUserJointAddAngularRow( priv->joint, 0.0f, &globalMatrixParent.x_ax[0] );
178 // NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
179 // NewtonUserJointAddAngularRow( priv->joint, 0.0f, &globalMatrixParent.y_ax[0] );
180 // NewtonUserJointSetRowStiffness( priv->joint, 0.98f );
181 
182 // NewtonUserJointAddAngularRow( priv->joint, 0.0f, &globalMatrixParent.z_ax[0] );
183 // NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
184  qChild = globalMatrixChild.w_pos + globalMatrixChild.x_ax.scale(len);
185  qParent = globalMatrixParent.w_pos + globalMatrixParent.x_ax.scale(len);
186  NewtonUserJointAddLinearRow( priv->joint, &qChild[0], &qParent[0], &globalMatrixParent.y_ax[0] );
187  NewtonUserJointSetRowStiffness( priv->joint, 1.0 );
188 
189  // check limits
190  if(dof->isLimited())
191  {
192  real lolimit;
193  real hilimit;
194  dof->limits(lolimit, hilimit);
195  if(pos < lolimit)
196  {
197 // printf("*** SUPERATI LIMITI GIUNTO ***\n");
198  wVector limitPos = globalMatrixParent.w_pos + globalMatrixParent.z_ax.scale(lolimit);
199  NewtonUserJointAddLinearRow( priv->joint, &childPos[0], &limitPos[0], &globalMatrixChild.z_ax[0] );
200  NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
201  return;
202  }
203  else if (pos > hilimit )
204  {
205 // printf("*** SUPERATI LIMITI GIUNTO ***\n");
206  wVector limitPos = globalMatrixParent.w_pos + globalMatrixParent.z_ax.scale(hilimit);
207  NewtonUserJointAddLinearRow( priv->joint, &childPos[0], &limitPos[0], &globalMatrixChild.z_ax[0] );
208  NewtonUserJointSetRowStiffness( priv->joint, 1.0f );
209  return;
210  }
211  }
212 
213  //--- if it reach this point means that the joint if far from limits
214  //--- and we check for type of motion and we'll apply the corresponding entity
215  real force, accel, mass;
216  switch( dof->motion() ) {
217  case PhyDOF::force:
218  //--- force will be used to set-up max and min torque newton will use to
219  //--- resolve the constraint and accel to the double of appliedForce
220  //--- this means that Newton will apply exactly forse amount of torque around
221  //--- the axis
222  force = dof->appliedForce();
223  if (dof->maxForce() > 0.0) {
224  if (force > dof->maxForce()) {
225  force = dof->maxForce();
226  } else if (force < -dof->maxForce()) {
227  force = -dof->maxForce();
228  }
229  }
230  NewtonUserJointAddLinearRow( priv->joint, &globalMatrixChild.w_pos[0], &globalMatrixParent.w_pos[0], &globalMatrixChild.z_ax[0] );
231  mass = child()->mass() + ((parent() != NULL) ? parent()->mass() : 0.0);
232  accel = force / mass;
233  NewtonUserJointSetRowAcceleration( priv->joint, accel );
234 // printf("--------- JOINT accel: %f, force: %f, mass: %f, maxForce: %f\n", accel, force, mass, dof->maxForce());
235  break;
236  case PhyDOF::vel:
237 // //--- I'm not sure that this implementation is correct
238 // accel = 0.8*( dof->desiredVelocity() - vel ) / ( timestep );
239 // wishangle = (dof->desiredVelocity()*timestep);
240 // NewtonUserJointAddAngularRow( priv->joint, wishangle, &globalMatrixChild.z_ax[0] );
241 // NewtonUserJointSetRowAcceleration( priv->joint, accel );
242 // NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
243 // if ( dof->maxForce() > 0.0 ) {
244 // NewtonUserJointSetRowMinimumFriction( priv->joint, -dof->maxForce() );
245 // NewtonUserJointSetRowMaximumFriction( priv->joint, +dof->maxForce() );
246 // }
247  break;
248  case PhyDOF::pos:
249 // wishangle = dof->desiredPosition() - angle;
250 // wishvel = wishangle / timestep;
251 // if ( fabs(wishvel) > dof->maxVelocity() ) {
252 // sign = (wishvel<0) ? -1.0 : +1.0;
253 // wishvel = sign*dof->maxVelocity();
254 // }
255 // accel = 0.8*( wishvel - vel ) / ( timestep );
256 // NewtonUserJointAddAngularRow( priv->joint, wishangle, &globalMatrixChild.z_ax[0] );
257 // NewtonUserJointSetRowAcceleration( priv->joint, accel );
258 // NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
259 // if ( dof->maxForce() > 0.0 ) {
260 // NewtonUserJointSetRowMinimumFriction( priv->joint, -dof->maxForce() );
261 // NewtonUserJointSetRowMaximumFriction( priv->joint, +dof->maxForce() );
262 // }
263 // //NewtonUserJointAddAngularRow( priv->joint, wishangle, &globalMatrixChild.z_ax[0] );
264 // //NewtonUserJointSetRowSpringDamperAcceleration( priv->joint, 30000.0, 2000 );
265 // //NewtonUserJointSetRowStiffness( priv->joint, dof->stiffness() );
266  break;
267  case PhyDOF::off:
268  default:
269  break;
270  }
271 
272  //--- Retrive forces applied to the joint by the constraints
273 #warning THIS IS NOT UPDATE IF THE JOINT REACHES THE LIMITS
274  forceOnJoint.x = NewtonUserJointGetRowForce (priv->joint, 0);
275  forceOnJoint.y = NewtonUserJointGetRowForce (priv->joint, 1);
276  forceOnJoint.z = NewtonUserJointGetRowForce (priv->joint, 2);
277 #endif
278 
279 }
280 
281 } // end namespace farsa
farsa::PhyDOF::limits
void limits(real &lo, real &hi) const
return the limits
Definition: phyjoint.h:155
farsa::PhySlider::globalMatrixParent
wMatrix globalMatrixParent
Global Matrix respect to Parent.
Definition: physlider.h:81
farsa::PhySlider::forceOnJoint
wVector forceOnJoint
The force applied to this joint.
Definition: physlider.h:91
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::PhySlider::globalMatrixChild
wMatrix globalMatrixChild
Global Matrix respect to Child.
Definition: physlider.h:87
farsa::wMatrix::inverse
wMatrix inverse() const
calculate the inverse of transformation matrix
Definition: wmatrix.h:258
farsa::PhyDOF::velocity
real velocity() const
return the actual relative velocity of bodies along the axis of rotation/translation ...
Definition: phyjoint.h:145
farsa::PhyObject::mass
real mass()
Return the mass.
Definition: phyobject.cpp:243
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::PhySlider::centre
virtual wVector centre() const
Return the centre of this joint in world coordinate.
Definition: physlider.cpp:67
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::PhySlider::updateJointInfo
virtual void updateJointInfo()
Update the Joint informations (each PhyDOF will be updated)
Definition: physlider.cpp:87
farsa::wMatrix::sanityCheck
bool sanityCheck()
return true if the transformation matrix is valid
Definition: wmatrix.h:431
farsa::PhySlider::localMatrixParent
wMatrix localMatrixParent
Local frame of the joint respect to the parent matrix.
Definition: physlider.h:67
farsa::PhySlider::createPrivateJoint
void createPrivateJoint()
Engine encapsulation.
Definition: physlider.cpp:80
farsa::PhySlider::getForceOnJoint
virtual wVector getForceOnJoint() const
Returns the force applied to this joint.
Definition: physlider.cpp:75
farsa::PhyJoint::dofsv
QVector< PhyDOF * > dofsv
vector of DOF's joint
Definition: phyjoint.h:434
farsa::PhySlider::localMatrixChild
wMatrix localMatrixChild
Local frame of the joint respect to the child matrix.
Definition: physlider.h:69
farsa::World::timeStep
real timeStep() const
get the time step setted
Definition: world.cpp:342
farsa::PhyJoint::priv
PhyJointPrivate * priv
Engine encapsulation.
Definition: phyjoint.h:451
farsa::PhyDOF::appliedForce
real appliedForce() const
Return the Force/Torque applied using applyForce.
Definition: phyjoint.h:98
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
farsa::real
float real
farsa::PhyDOF::maxForce
real maxForce() const
returns the maximum force/torque.
Definition: phyjoint.h:188
farsa::PhyDOF::motion
MotionMode motion() const
return the actual motion mode
Definition: phyjoint.h:113
farsa::PhyJoint::enabled
bool enabled
true if is enabled, false otherwise
Definition: phyjoint.h:442
farsa::PhyDOF
PhyDOF class.
Definition: phyjoint.h:50
farsa::PhyJoint::child
virtual PhyObject * child()
Return the child object attached to this joint (see Newton Documentation)
Definition: phyjoint.h:371
farsa::PhySlider::PhySlider
PhySlider(const wVector &axis, PhyObject *parent, PhyObject *child, bool cp=true)
Constructor.
Definition: physlider.cpp:32
farsa::PhyObject
PhyObject class.
Definition: phyobject.h:46
farsa::PhyDOF::isLimited
bool isLimited() const
return true if rotation/translation are limited
Definition: phyjoint.h:230