openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
kinbodylink.cpp
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2012 Rosen Diankov (rosen.diankov@gmail.com)
3 //
4 // This file is part of OpenRAVE.
5 // OpenRAVE is free software: you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation, either version 3 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 Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
17 #include "libopenrave.h"
18 #include <algorithm>
19 
20 namespace OpenRAVE {
21 
22 KinBody::LinkInfo::LinkInfo() : XMLReadable("link"), _mass(0), _bStatic(false), _bIsEnabled(true) {
23 }
24 
26 {
27  _parent = parent;
28  _index = -1;
29 }
30 
32 {
33 }
34 
35 
36 void KinBody::Link::Enable(bool bEnable)
37 {
38  if( _info._bIsEnabled != bEnable ) {
39  KinBodyPtr parent = GetParent();
40  parent->_nNonAdjacentLinkCache &= ~AO_Enabled;
41  _info._bIsEnabled = bEnable;
42  GetParent()->_ParametersChanged(Prop_LinkEnable);
43  }
44 }
45 
47 {
48  return _info._bIsEnabled;
49 }
50 
51 bool KinBody::Link::SetVisible(bool visible)
52 {
53  bool bchanged = false;
54  FOREACH(itgeom,_vGeometries) {
55  if( (*itgeom)->_info._bVisible != visible ) {
56  (*itgeom)->_info._bVisible = visible;
57  bchanged = true;
58  }
59  }
60  if( bchanged ) {
61  GetParent()->_ParametersChanged(Prop_LinkDraw);
62  return true;
63  }
64  return false;
65 }
66 
68 {
69  FOREACHC(itgeom,_vGeometries) {
70  if( (*itgeom)->IsVisible() ) {
71  return true;
72  }
73  }
74  return false;
75 }
76 
77 void KinBody::Link::GetParentLinks(std::vector< boost::shared_ptr<Link> >& vParentLinks) const
78 {
79  KinBodyConstPtr parent(_parent);
80  vParentLinks.resize(_vParentLinks.size());
81  for(size_t i = 0; i < _vParentLinks.size(); ++i) {
82  vParentLinks[i] = parent->GetLinks().at(_vParentLinks[i]);
83  }
84 }
85 
86 bool KinBody::Link::IsParentLink(boost::shared_ptr<Link const> plink) const
87 {
88  return find(_vParentLinks.begin(),_vParentLinks.end(),plink->GetIndex()) != _vParentLinks.end();
89 }
90 
100 static TransformMatrix ComputeInertia(const Transform& tMassFrame, const Vector& vinertiamoments)
101 {
102  TransformMatrix minertia;
103  dReal i0 = vinertiamoments[0], i1 = vinertiamoments[1], i2 = vinertiamoments[2];
104  dReal q0=tMassFrame.rot[0], q1=tMassFrame.rot[1], q2=tMassFrame.rot[2], q3=tMassFrame.rot[3];
105  dReal q1_2 = q1*q1, q2_2 = q2*q2, q3_2 = q3*q3;
106  minertia.m[0] = i0*utils::Sqr(1 - 2*q2_2 - 2*q3_2) + i1*utils::Sqr(-2*q0*q3 + 2*q1*q2) + i2*utils::Sqr(2*q0*q2 + 2*q1*q3);
107  minertia.m[1] = i0*(2*q0*q3 + 2*q1*q2)*(1 - 2*q2_2 - 2*q3_2) + i1*(-2*q0*q3 + 2*q1*q2)*(1 - 2*q1_2 - 2*q3_2) + i2*(-2*q0*q1 + 2*q2*q3)*(2*q0*q2 + 2*q1*q3);
108  minertia.m[2] = i0*(-2*q0*q2 + 2*q1*q3)*(1 - 2*q2_2 - 2*q3_2) + i1*(-2*q0*q3 + 2*q1*q2)*(2*q0*q1 + 2*q2*q3) + i2*(2*q0*q2 + 2*q1*q3)*(1 - 2*q1_2 - 2*q2_2);
109  minertia.m[3] = 0;
110  minertia.m[4] = minertia.m[1];
111  minertia.m[5] = i0*utils::Sqr(2*q0*q3 + 2*q1*q2) + i1*utils::Sqr(1 - 2*q1_2 - 2*q3_2) + i2*utils::Sqr(-2*q0*q1 + 2*q2*q3);
112  minertia.m[6] = i0*(-2*q0*q2 + 2*q1*q3)*(2*q0*q3 + 2*q1*q2) + i1*(2*q0*q1 + 2*q2*q3)*(1 - 2*q1_2 - 2*q3_2) + i2*(-2*q0*q1 + 2*q2*q3)*(1 - 2*q1_2 - 2*q2_2);
113  minertia.m[7] = 0;
114  minertia.m[8] = minertia.m[2];
115  minertia.m[9] = minertia.m[6];
116  minertia.m[10] = i0*utils::Sqr(-2*q0*q2 + 2*q1*q3) + i1*utils::Sqr(2*q0*q1 + 2*q2*q3) + i2*utils::Sqr(1 - 2*q1_2 - 2*q2_2);
117  minertia.m[11] = 0;
118  return minertia;
119 }
121 {
122  return ComputeInertia(_info._tMassFrame, _info._vinertiamoments);
123 }
124 
126 {
127  return ComputeInertia(_info._t*_info._tMassFrame, _info._vinertiamoments);
128 }
129 
131 {
132  _info._tMassFrame=massframe;
133  GetParent()->_ParametersChanged(Prop_LinkDynamics);
134 }
135 
137 {
138  _info._vinertiamoments = inertiamoments;
139  GetParent()->_ParametersChanged(Prop_LinkDynamics);
140 }
141 
143 {
144  _info._mass=mass;
145  GetParent()->_ParametersChanged(Prop_LinkDynamics);
146 }
147 
149 {
150  if( _vGeometries.size() == 1) {
151  return _vGeometries.front()->ComputeAABB(Transform());
152  }
153  else if( _vGeometries.size() > 1 ) {
154  Vector vmin, vmax;
155  bool binitialized=false;
156  AABB ab;
157  FOREACHC(itgeom,_vGeometries) {
158  ab = (*itgeom)->ComputeAABB(Transform());
159  if( ab.extents.x <= 0 || ab.extents.y <= 0 || ab.extents.z <= 0 ) {
160  continue;
161  }
162  Vector vnmin = ab.pos - ab.extents;
163  Vector vnmax = ab.pos + ab.extents;
164  if( !binitialized ) {
165  vmin = vnmin;
166  vmax = vnmax;
167  binitialized = true;
168  }
169  else {
170  if( vmin.x > vnmin.x ) {
171  vmin.x = vnmin.x;
172  }
173  if( vmin.y > vnmin.y ) {
174  vmin.y = vnmin.y;
175  }
176  if( vmin.z > vnmin.z ) {
177  vmin.z = vnmin.z;
178  }
179  if( vmax.x < vnmax.x ) {
180  vmax.x = vnmax.x;
181  }
182  if( vmax.y < vnmax.y ) {
183  vmax.y = vnmax.y;
184  }
185  if( vmax.z < vnmax.z ) {
186  vmax.z = vnmax.z;
187  }
188  }
189  }
190  if( !binitialized ) {
191  ab.pos = _info._t.trans;
192  ab.extents = Vector(0,0,0);
193  }
194  else {
195  ab.pos = (dReal)0.5 * (vmin + vmax);
196  ab.extents = vmax - ab.pos;
197  }
198  return ab;
199  }
200  return AABB();
201 }
202 
204 {
205  if( _vGeometries.size() == 1) {
206  return _vGeometries.front()->ComputeAABB(_info._t);
207  }
208  else if( _vGeometries.size() > 1 ) {
209  Vector vmin, vmax;
210  bool binitialized=false;
211  AABB ab;
212  FOREACHC(itgeom,_vGeometries) {
213  ab = (*itgeom)->ComputeAABB(_info._t);
214  if( ab.extents.x <= 0 || ab.extents.y <= 0 || ab.extents.z <= 0 ) {
215  continue;
216  }
217  Vector vnmin = ab.pos - ab.extents;
218  Vector vnmax = ab.pos + ab.extents;
219  if( !binitialized ) {
220  vmin = vnmin;
221  vmax = vnmax;
222  binitialized = true;
223  }
224  else {
225  if( vmin.x > vnmin.x ) {
226  vmin.x = vnmin.x;
227  }
228  if( vmin.y > vnmin.y ) {
229  vmin.y = vnmin.y;
230  }
231  if( vmin.z > vnmin.z ) {
232  vmin.z = vnmin.z;
233  }
234  if( vmax.x < vnmax.x ) {
235  vmax.x = vnmax.x;
236  }
237  if( vmax.y < vnmax.y ) {
238  vmax.y = vnmax.y;
239  }
240  if( vmax.z < vnmax.z ) {
241  vmax.z = vnmax.z;
242  }
243  }
244  }
245  if( !binitialized ) {
246  ab.pos = _info._t.trans;
247  ab.extents = Vector(0,0,0);
248  }
249  else {
250  ab.pos = (dReal)0.5 * (vmin + vmax);
251  ab.extents = vmax - ab.pos;
252  }
253  return ab;
254  }
255  // have to at least return the correct position!
256  return AABB(_info._t.trans,Vector(0,0,0));
257 }
258 
259 void KinBody::Link::serialize(std::ostream& o, int options) const
260 {
261  o << _index << " ";
262  if( options & SO_Geometry ) {
263  o << _vGeometries.size() << " ";
264  FOREACHC(it,_vGeometries) {
265  (*it)->serialize(o,options);
266  }
267  }
268  if( options & SO_Dynamics ) {
269  SerializeRound(o,_info._tMassFrame);
270  SerializeRound(o,_info._mass);
271  SerializeRound3(o,_info._vinertiamoments);
272  }
273 }
274 
275 void KinBody::Link::SetStatic(bool bStatic)
276 {
277  if( _info._bStatic != bStatic ) {
278  _info._bStatic = bStatic;
279  GetParent()->_ParametersChanged(Prop_LinkStatic);
280  }
281 }
282 
284 {
285  _info._t = t;
286  GetParent()->_nUpdateStampId++;
287 }
288 
289 void KinBody::Link::SetForce(const Vector& force, const Vector& pos, bool bAdd)
290 {
291  GetParent()->GetEnv()->GetPhysicsEngine()->SetBodyForce(shared_from_this(), force, pos, bAdd);
292 }
293 
294 void KinBody::Link::SetTorque(const Vector& torque, bool bAdd)
295 {
296  GetParent()->GetEnv()->GetPhysicsEngine()->SetBodyTorque(shared_from_this(), torque, bAdd);
297 }
298 
299 void KinBody::Link::SetVelocity(const Vector& linearvel, const Vector& angularvel)
300 {
301  GetParent()->GetEnv()->GetPhysicsEngine()->SetLinkVelocity(shared_from_this(), linearvel, angularvel);
302 }
303 
304 void KinBody::Link::GetVelocity(Vector& linearvel, Vector& angularvel) const
305 {
306  GetParent()->GetEnv()->GetPhysicsEngine()->GetLinkVelocity(shared_from_this(), linearvel, angularvel);
307 }
308 
310 std::pair<Vector,Vector> KinBody::Link::GetVelocity() const
311 {
312  std::pair<Vector,Vector> velocities;
313  GetParent()->GetEnv()->GetPhysicsEngine()->GetLinkVelocity(shared_from_this(), velocities.first, velocities.second);
314  return velocities;
315 }
316 
318 {
319  return _vGeometries.at(index);
320 }
321 
322 void KinBody::Link::InitGeometries(std::vector<KinBody::GeometryInfoConstPtr>& geometries)
323 {
324  _vGeometries.resize(geometries.size());
325  for(size_t i = 0; i < geometries.size(); ++i) {
326  _vGeometries[i].reset(new Geometry(shared_from_this(),*geometries[i]));
327  }
328  _Update();
329  GetParent()->_ParametersChanged(Prop_LinkGeometry);
330 }
331 
332 void KinBody::Link::InitGeometries(std::list<KinBody::GeometryInfo>& geometries)
333 {
334  _vGeometries.resize(geometries.size());
335  size_t i = 0;
336  FOREACH(itinfo,geometries) {
337  _vGeometries[i].reset(new Geometry(shared_from_this(),*itinfo));
338  ++i;
339  }
340  _Update();
341  GetParent()->_ParametersChanged(Prop_LinkGeometry);
342 }
343 
344 void KinBody::Link::SwapGeometries(boost::shared_ptr<Link>& link)
345 {
346  _vGeometries.swap(link->_vGeometries);
347  FOREACH(itgeom,_vGeometries) {
348  (*itgeom)->_parent = shared_from_this();
349  }
350  FOREACH(itgeom,link->_vGeometries) {
351  (*itgeom)->_parent = link;
352  }
353  _Update();
354  link->_Update();
355  GetParent()->_ParametersChanged(Prop_LinkGeometry);
356  link->GetParent()->_ParametersChanged(Prop_LinkGeometry);
357 }
358 
359 bool KinBody::Link::ValidateContactNormal(const Vector& position, Vector& normal) const
360 {
361  if( _vGeometries.size() == 1) {
362  return _vGeometries.front()->ValidateContactNormal(position,normal);
363  }
364  else if( _vGeometries.size() > 1 ) {
365  RAVELOG_VERBOSE(str(boost::format("cannot validate normal when there is more than one geometry in link '%s(%d)' (do not know colliding geometry)")%_info._name%GetIndex()));
366  }
367  return false;
368 }
369 
370 void KinBody::Link::GetRigidlyAttachedLinks(std::vector<boost::shared_ptr<Link> >& vattachedlinks) const
371 {
372  KinBodyPtr parent(_parent);
373  vattachedlinks.resize(0);
374  FOREACHC(it, _vRigidlyAttachedLinks) {
375  vattachedlinks.push_back(parent->GetLinks().at(*it));
376  }
377 }
378 
379 void KinBody::Link::SetFloatParameters(const std::string& key, const std::vector<dReal>& parameters)
380 {
381  if( parameters.size() > 0 ) {
382  _info._mapFloatParameters[key] = parameters;
383  }
384  else {
385  _info._mapFloatParameters.erase(key);
386  }
387  GetParent()->_ParametersChanged(Prop_LinkCustomParameters);
388 }
389 
390 void KinBody::Link::SetIntParameters(const std::string& key, const std::vector<int>& parameters)
391 {
392  if( parameters.size() > 0 ) {
393  _info._mapIntParameters[key] = parameters;
394  }
395  else {
396  _info._mapIntParameters.erase(key);
397  }
398  GetParent()->_ParametersChanged(Prop_LinkCustomParameters);
399 }
400 
401 bool KinBody::Link::IsRigidlyAttached(boost::shared_ptr<Link const> plink) const
402 {
403  return find(_vRigidlyAttachedLinks.begin(),_vRigidlyAttachedLinks.end(),plink->GetIndex()) != _vRigidlyAttachedLinks.end();
404 }
405 
407 {
408  if( _info._vgeometryinfos.size() != _vGeometries.size() ) {
409  // have to recompute the geometries
410  _info._vgeometryinfos.resize(_vGeometries.size());
411  for(size_t i = 0; i < _info._vgeometryinfos.size(); ++i) {
412  if( !_info._vgeometryinfos[i] ) {
413  _info._vgeometryinfos[i].reset(new KinBody::GeometryInfo());
414  }
415  *_info._vgeometryinfos[i] = _vGeometries[i]->GetInfo();
416  }
417  }
418 }
419 
421 {
422  _collision.vertices.resize(0);
423  _collision.indices.resize(0);
424  FOREACH(itgeom,_vGeometries) {
425  _collision.Append((*itgeom)->GetCollisionMesh(),(*itgeom)->GetTransform());
426  }
427  GetParent()->_ParametersChanged(Prop_LinkGeometry);
428 }
429 
430 }