ODE_Link.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
9 
10 #include "ODE_Link.h"
11 
13  const dReal* R = dBodyGetRotation(bodyId);
14  R_ << R[0],R[1],R[2],
15  R[4],R[5],R[6],
16  R[8],R[9],R[10];
17  dVector3 result;
18  dBodyGetRelPointPos(bodyId, -C[0], -C[1], -C[2], result);
19  pos_ << result[0], result[1], result[2];
20 
21 }
22 
24  hrp::Vector3 _pos(R * C + pos);
25  dBodySetPosition(bodyId, _pos(0), _pos(1), _pos(2));
26  dMatrix3 _R = {R(0,0), R(0,1), R(0,2), 0,
27  R(1,0), R(1,1), R(1,2), 0,
28  R(2,0), R(2,1), R(2,2), 0};
29  dBodySetRotation(bodyId, _R);
30 }
31 
34  return dJointGetHingeAngle(odeJointId);
36  return dJointGetSliderPosition(odeJointId);
37  else
38  return 0;
39 }
40 
43  return dJointGetHingeAngleRate(odeJointId);
45  return dJointGetSliderPositionRate(odeJointId);
46  else
47  return 0;
48 }
49 
52  return dJointAddHingeTorque(odeJointId, t);
54  return dJointAddSliderForce(odeJointId, t);
55 }
56 
58  return dBodyGetAngularVel(bodyId);
59 }
60 
62  dVector3 result;
63  dBodyGetRelPointVel(bodyId, -C[0], -C[1], -C[2], result);
64  v << result[0], result[1], result[2];
65 }
66 
68  dBodySetAngularVel(bodyId, w[0], w[1], w[2]);
71  getTransform(p, R);
72  hrp::Vector3 cpos(R*C);
73  hrp::Vector3 _v(v + w.cross(cpos));
74  dBodySetLinearVel(bodyId, _v[0], _v[1], _v[2]);
75 }
76 
78  return dBodyGetForce(bodyId);
79 }
80 
82  return dBodyGetTorque(bodyId);
83 }
84 
85 void ODE_Link::setForce(double fx, double fy, double fz){
86  dBodyAddForce(bodyId, fx, fy, fz);
87 }
88 
89 void ODE_Link::setTorque(double tx, double ty, double tz){
90  dBodyAddTorque(bodyId, tx, ty, tz);
91 }
92 
94 {
96  dJointDestroy(odeJointId);
97  for(int i=0; i<geomIds.size(); i++)
98  dGeomDestroy(geomIds.at(i));
99  if(triMeshDataId)
100  dGeomTriMeshDataDestroy(triMeshDataId);
101  dBodyDestroy(bodyId);
102  ODE_Link* link = static_cast<ODE_Link*>(child);
103  while(link){
104  ODE_Link* linkToDelete = (ODE_Link*)link;
105  link = static_cast<ODE_Link*>(link->sibling);
106  linkToDelete->destroy();
107  }
108 }
109 
110 
i
png_uint_32 i
Definition: png.h:2732
ColladaUtil::dReal
double dReal
Definition: ColladaUtil.h:78
hrp::Vector3
Eigen::Vector3d Vector3
Definition: EigenTypes.h:11
hrp::Matrix33
Eigen::Matrix3d Matrix33
Definition: EigenTypes.h:12


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:03