ODE_Link.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  */
00009 
00010 #include "ODE_Link.h"
00011 
00012 void ODE_Link::getTransform(hrp::Vector3& pos_, hrp::Matrix33& R_){
00013     const dReal* R = dBodyGetRotation(bodyId);
00014     R_ << R[0],R[1],R[2],
00015           R[4],R[5],R[6],
00016           R[8],R[9],R[10];
00017     dVector3 result;
00018     dBodyGetRelPointPos(bodyId, -C[0], -C[1], -C[2], result);
00019     pos_ << result[0], result[1], result[2];
00020 
00021 }
00022 
00023 void ODE_Link::setTransform(const hrp::Vector3& pos, const hrp::Matrix33& R){
00024     hrp::Vector3 _pos(R * C + pos);
00025     dBodySetPosition(bodyId, _pos(0), _pos(1), _pos(2));
00026     dMatrix3 _R = {R(0,0), R(0,1), R(0,2), 0,
00027                   R(1,0), R(1,1), R(1,2), 0,
00028                   R(2,0), R(2,1), R(2,2), 0};
00029     dBodySetRotation(bodyId, _R);
00030 }
00031 
00032 dReal ODE_Link::getAngle(){
00033     if(jointType == ODE_Link::ROTATIONAL_JOINT)
00034         return dJointGetHingeAngle(odeJointId);
00035     else if(jointType == ODE_Link::SLIDE_JOINT)
00036         return dJointGetSliderPosition(odeJointId);
00037     else
00038         return 0;
00039 }
00040 
00041 dReal ODE_Link::getVelocity(){
00042     if(jointType == ODE_Link::ROTATIONAL_JOINT)
00043         return dJointGetHingeAngleRate(odeJointId);
00044     else if(jointType == ODE_Link::SLIDE_JOINT)
00045         return dJointGetSliderPositionRate(odeJointId);
00046     else
00047         return 0;
00048 }
00049 
00050 void ODE_Link::setTorque(dReal t){
00051     if(jointType == ODE_Link::ROTATIONAL_JOINT)
00052         return dJointAddHingeTorque(odeJointId, t);
00053     else if(jointType == ODE_Link::SLIDE_JOINT)
00054         return dJointAddSliderForce(odeJointId, t);
00055 }
00056 
00057 const dReal* ODE_Link::getAngularVel(){
00058     return dBodyGetAngularVel(bodyId);
00059 }
00060 
00061 void ODE_Link::getLinearVel(hrp::Vector3& v){
00062     dVector3 result;
00063     dBodyGetRelPointVel(bodyId, -C[0], -C[1], -C[2], result);
00064     v << result[0], result[1], result[2];
00065 }
00066 
00067 void ODE_Link::setAbsVelocity(hrp::Vector3& v, hrp::Vector3& w){
00068     dBodySetAngularVel(bodyId, w[0], w[1], w[2]);
00069     hrp::Vector3 p;
00070     hrp::Matrix33 R;
00071     getTransform(p, R);
00072     hrp::Vector3 cpos(R*C);
00073     hrp::Vector3 _v(v + w.cross(cpos));
00074     dBodySetLinearVel(bodyId, _v[0], _v[1], _v[2]);
00075 }
00076 
00077 const dReal* ODE_Link::getForce(){
00078     return dBodyGetForce(bodyId);
00079 }
00080 
00081 const dReal* ODE_Link::getTorque(){
00082     return dBodyGetTorque(bodyId);
00083 }
00084 
00085 void ODE_Link::setForce(double fx, double fy, double fz){
00086     dBodyAddForce(bodyId, fx, fy, fz);
00087 }
00088 
00089 void ODE_Link::setTorque(double tx, double ty, double tz){
00090     dBodyAddTorque(bodyId, tx, ty, tz);
00091 }
00092 
00093 void ODE_Link::destroy()
00094 {
00095     if(jointType!=FREE_JOINT)
00096         dJointDestroy(odeJointId);
00097     for(int i=0; i<geomIds.size(); i++)
00098         dGeomDestroy(geomIds.at(i));
00099     if(triMeshDataId)
00100         dGeomTriMeshDataDestroy(triMeshDataId);
00101     dBodyDestroy(bodyId);
00102     ODE_Link* link = static_cast<ODE_Link*>(child);
00103     while(link){
00104         ODE_Link* linkToDelete = (ODE_Link*)link;
00105         link = static_cast<ODE_Link*>(link->sibling);
00106         linkToDelete->destroy();
00107     }
00108 }
00109 
00110 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:18