int w_Joint_getType(lua_State *L)
{
Joint *t = luax_checkjoint(L, 1);
const char *type = "";
Joint::getConstant(t->getType(), type);
lua_pushstring(L, type);
return 1;
}
void Skeleton::setJoint(Joint joint) {
string jointType = joint.getType();
if (jointType == "ThumbRight") {
setThumbRight(joint);
} else if (jointType == "SpineBase") {
setSpineBase(joint);
} else if (jointType == "SpineMid") {
setSpineMid(joint);
} else if (jointType == "Neck") {
setNeck(joint);
} else if (jointType == "Head") {
setHead(joint);
} else if (jointType == "ShoulderLeft") {
setShoulderLeft(joint);
} else if (jointType == "ElbowLeft") {
setElbowLeft(joint);
} else if (jointType == "WristLeft") {
setWristLeft(joint);
} else if (jointType == "HandLeft") {
setHandLeft(joint);
} else if (jointType == "ShoulderRight") {
setShoulderRight(joint);
} else if (jointType == "ElbowRight") {
setElbowRight(joint);
} else if (jointType == "WristRight") {
setWristRight(joint);
} else if (jointType == "HandRight") {
setHandRight(joint);
} else if (jointType == "HipLeft") {
setHipLeft(joint);
} else if (jointType == "KneeLeft") {
setKneeLeft(joint);
} else if (jointType == "AnkleLeft") {
setAnkleLeft(joint);
} else if (jointType == "FootLeft") {
setFootLeft(joint);
} else if (jointType == "HipRight") {
setHipRight(joint);
} else if (jointType == "KneeRight") {
setKneeRight(joint);
} else if (jointType == "AnkleRight") {
setAnkleRight(joint);
} else if (jointType == "FootRight") {
setFootRight(joint);
} else if (jointType == "SpineShoulder") {
setSpineShoulder(joint);
} else if (jointType == "HandTipLeft") {
setHandTipLeft(joint);
} else if (jointType == "ThumbLeft") {
setThumbLeft(joint);
} else if (jointType == "HandTipRight") {
setHandTipRight(joint);
}
}
void PlaneRegistration::init_kdl_robot()
{
std::string urdf;
node_->param("/robot_description", urdf, std::string());
urdf::Model urdf_model;
urdf_model.initString(urdf);
// get tree from urdf string
KDL::Tree my_tree;
if (!kdl_parser::treeFromString(urdf, my_tree)) {
ROS_ERROR("Failed to construct kdl tree");
return;
}
std::string rootLink = "wam/base_link";
std::string tipLink = "wam/cutter_tip_link";
if (!my_tree.getChain(rootLink, tipLink, robot_))
{
ROS_ERROR("Failed to get chain from kdl tree, check root/rip link");
return;
}
num_jnts_ = robot_.getNrOfJoints();
// resize joint states
jnt_pos_.resize(num_jnts_);
jnt_vel_.resize(num_jnts_);
// jnt_cmd_.resize(num_jnts_);
jnt_limit_min_.resize(num_jnts_);
jnt_limit_max_.resize(num_jnts_);
// get jnt limits from URDF model
size_t i_jnt = 0;
for (size_t i = 0; i < robot_.getNrOfSegments(); i++) {
Joint jnt = robot_.getSegment(i).getJoint();
if (jnt.getType() != Joint::None) {
jnt_limit_min_(i_jnt) = urdf_model.joints_[jnt.getName()]->limits->lower;
jnt_limit_max_(i_jnt) = urdf_model.joints_[jnt.getName()]->limits->upper;
i_jnt++;
}
}
// print limit for debugging
std::cout << "min: " << jnt_limit_min_.data.transpose() << std::endl;
std::cout << "max: " << jnt_limit_max_.data.transpose() << std::endl;
// KDL Solvers
fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(robot_));
ik_solver_.reset(
new KDL::ChainIkSolverPos_LMA(
robot_,
1E-5,
500,
1E-15));
}
void KinFamTest::JointTest()
{
double q;
Joint j;
j=Joint("Joint 1", Joint::None);
CPPUNIT_ASSERT_EQUAL(Joint::None,j.getType());
random(q);
CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame::Identity());
random(q);
CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist::Zero());
random(q);
j=Joint("Joint 2", Joint::RotX);
CPPUNIT_ASSERT_EQUAL(Joint::RotX,j.getType());
CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotX(q)));
random(q);
CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(q,0,0)));
random(q);
j=Joint("Joint 3", Joint::RotY);
CPPUNIT_ASSERT_EQUAL(Joint::RotY,j.getType());
CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotY(q)));
random(q);
CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,q,0)));
random(q);
j=Joint("Joint 4", Joint::RotZ);
CPPUNIT_ASSERT_EQUAL(Joint::RotZ,j.getType());
CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotZ(q)));
random(q);
CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,0,q)));
random(q);
j=Joint("Joint 5", Joint::TransX);
CPPUNIT_ASSERT_EQUAL(Joint::TransX,j.getType());
CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(q,0,0)));
random(q);
CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(q,0,0),Vector::Zero()));
random(q);
j=Joint("Joint 6", Joint::TransY);
CPPUNIT_ASSERT_EQUAL(Joint::TransY,j.getType());
CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,q,0)));
random(q);
CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(0,q,0),Vector::Zero()));
random(q);
j=Joint("Joint 7", Joint::TransZ);
CPPUNIT_ASSERT_EQUAL(Joint::TransZ,j.getType());
CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,0,q)));
random(q);
CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(0,0,q),Vector::Zero()));
}
/*!
Computes the jacobian for the base frame of link \a l on finger \a f
relative to the base frame of the finger. This is wrt THE FRAME OF LINK!!!
Distances USED TO BE converted to meters (check why this is...). Now they are
kept in MILLIMETERS!
*/
double *
Grasp::getLinkJacobian(int f, int l)
{
int j,col;
Joint *jointPtr;
int numDOF = hand->getNumDOF();
double *jac = new double[6*numDOF];
double k;
mat3 m;
vec3 p;
transf T;
double db0 = 0.0;
dcopy(6*numDOF,&db0,0,jac,1);
//I use f=-1 on virtual contacts to show that a contact is on the palm
if (f < 0) return jac;
for (j=hand->getFinger(f)->getLastJoint(l);j>=0;j--) {
jointPtr = hand->getFinger(f)->getJoint(j);
col = jointPtr->getDOFNum();
k = hand->getDOF(jointPtr->getDOFNum())->getStaticRatio(jointPtr);
T = T * jointPtr->getDH()->getTran();
m = T.affine();
p = T.translation();
if (jointPtr->getType() == REVOLUTE) {
jac[col*6] += k*(-m.element(0,0)*p.y() + m.element(0,1)*p.x());
jac[col*6+1] += k*(-m.element(1,0)*p.y() + m.element(1,1)*p.x());
jac[col*6+2] += k*(-m.element(2,0)*p.y() + m.element(2,1)*p.x());
jac[col*6+3] += k*m.element(0,2);
jac[col*6+4] += k*m.element(1,2);
jac[col*6+5] += k*m.element(2,2);
} else {
jac[col*6] += k*m.element(0,2);
jac[col*6+1] += k*m.element(1,2);
jac[col*6+2] += k*m.element(2,2);
jac[col*6+3] += 0.0;
jac[col*6+4] += 0.0;
jac[col*6+5] += 0.0;
}
}
return jac;
}
bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const
{
// clear chain
chain = Chain();
// walk down from chain_root and chain_tip to the root of the tree
vector<SegmentMap::key_type> parents_chain_root, parents_chain_tip;
for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s=s->second.parent){
parents_chain_root.push_back(s->first);
if (s->first == root_name) break;
}
if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false;
for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s=s->second.parent){
parents_chain_tip.push_back(s->first);
if (s->first == root_name) break;
}
if (parents_chain_tip.empty() || parents_chain_tip.back() != root_name) return false;
// remove common part of segment lists
SegmentMap::key_type last_segment = root_name;
while (!parents_chain_root.empty() && !parents_chain_tip.empty() &&
parents_chain_root.back() == parents_chain_tip.back()){
last_segment = parents_chain_root.back();
parents_chain_root.pop_back();
parents_chain_tip.pop_back();
}
parents_chain_root.push_back(last_segment);
// add the segments from the root to the common frame
for (unsigned int s=0; s<parents_chain_root.size()-1; s++){
Segment seg = getSegment(parents_chain_root[s])->second.segment;
Frame f_tip = seg.pose(0.0).Inverse();
Joint jnt = seg.getJoint();
if (jnt.getType() == Joint::RotX || jnt.getType() == Joint::RotY || jnt.getType() == Joint::RotZ || jnt.getType() == Joint::RotAxis)
jnt = Joint(jnt.getName(), f_tip*jnt.JointOrigin(), f_tip.M*jnt.JointAxis(), Joint::RotAxis);
else if (jnt.getType() == Joint::TransX || jnt.getType() == Joint::TransY || jnt.getType() == Joint::TransZ || jnt.getType() == Joint::TransAxis)
jnt = Joint(jnt.getName(),f_tip*jnt.JointOrigin(), f_tip.M*jnt.JointAxis(), Joint::TransAxis);
chain.addSegment(Segment(getSegment(parents_chain_root[s+1])->second.segment.getName(),
jnt, f_tip, getSegment(parents_chain_root[s+1])->second.segment.getInertia()));
}
// add the segments from the common frame to the tip frame
for (int s=parents_chain_tip.size()-1; s>-1; s--){
chain.addSegment(getSegment(parents_chain_tip[s])->second.segment);
}
return true;
}
void internal_collisionCallback(void *data, dGeomID o0, dGeomID o1)
{
if (dGeomIsSpace(o0) || dGeomIsSpace(o1))
{
// Colliding a space with either a geom or another space.
dSpaceCollide2(o0, o1, data, &internal_collisionCallback);
if (dGeomIsSpace(o0))
{
// Colliding all geoms internal to the space.
dSpaceCollide((dSpaceID) o0, data,
&internal_collisionCallback);
}
if (dGeomIsSpace(o1))
{
// Colliding all geoms internal to the space.
dSpaceCollide((dSpaceID) o1, data,
&internal_collisionCallback);
}
}
else
{
// Colliding two geoms.
// The following is a set of special cases where we might
// want to skip collision detection (but not all are
// enforced here for various reasons):
// 1. Two static Solids (neither geom has a body) AND
// neither Solid has a CollisionEventHandler AND there are
// not global handlers: this is enforced.
// 2. Two Shapes that are part of the same Solid (they
// share a body): this is not enforced because ODE
// already takes care of it.
// 3. Two sleeping Solids (note: both must have bodies to
// check this): this is enforced. (ODE should handle
// this, but it doesn't.)
// 4. Two Solids connected by a fixed Joint: this is
// enforced.
// 5. Two Solids connected by a Joint (besides ODE
// contact joints, which should never generate more
// contacts) with contacts disabled (note: both must have
// bodies to check this): this is enforced.
// 6. Solid0 is static, Solid1 is dynamic and is sleeping,
// static-to-sleeping contacts are ignored by the
// Simulator, and neither Solid has a
// CollisionEventHandler AND there are no global handlers:
// this is enforced.
// 7. Solid1 is static, Solid0 is dynamic and is sleeping,
// static-to-sleeping contacts are ignored by the
// Simulator, and neither Solid has a
// CollisionEventHandler AND there are no global handlers:
// this is enforced.
// 8. The two Solids' contact groups do not generate
// contacts when they collide AND neither Solid has a
// CollisionEventHandler AND there are no global handlers.
// Get the geoms' ODE body IDs.
dBodyID o0BodyID = dGeomGetBody(o0);
dBodyID o1BodyID = dGeomGetBody(o1);
bool solid0Static = (0 == o0BodyID);
bool solid1Static = (0 == o1BodyID);
// Check if both Solids are dynamic (i.e. have ODE bodies).
bool bothHaveBodies = true;
if (0 == o0BodyID || 0 == o1BodyID)
{
bothHaveBodies = false;
}
// If the two Solids are connected by a common Joint, get
// a pointer to that Joint.
Joint* commonJoint = NULL;
if (bothHaveBodies && dAreConnectedExcluding(o0BodyID,
o1BodyID, dJointTypeContact))
{
// This will become non-NULL only if there exists an ODE
// joint connecting the two bodies.
commonJoint = internal_getCommonJoint(o0BodyID, o1BodyID);
}
// Get pointers to the geoms' GeomData structures.
GeomData* geomData0 = (GeomData*) dGeomGetData(o0);
GeomData* geomData1 = ((GeomData*) dGeomGetData(o1));
// Get pointers to the geoms' ShapeData structures.
const ShapeData* shape0 = geomData0->shape;
const ShapeData* shape1 = geomData1->shape;
// Get a pointer to the ODESimulator.
ODESimulator* sim = (ODESimulator*)data;
// Check if the two Solids' contact groups generate contacts
// when they collide.
bool makeContacts = sim->groupsMakeContacts(
shape0->contactGroup, shape1->contactGroup);
// Find out whether the Simulator has static-to-sleeping
// contacts disabled.
bool ignoreStaticSleepingContacts =
!sim->areStaticSleepingContactsEnabled();
// Get pointers to the geoms' Solids.
Solid* solid0 = geomData0->solid;
Solid* solid1 = geomData1->solid;
// Get pointers to the two Solids' CollisionEventHandlers.
// These will be NULL if the Solids don't use
// CollisionEventHandlers.
CollisionEventHandler* handler0 =
solid0->getCollisionEventHandler();
CollisionEventHandler* handler1 =
solid1->getCollisionEventHandler();
bool neitherHasCollisionHandler = !(handler0 || handler1);
bool noGlobalCollisionHandlers =
sim->getNumGlobalCollisionEventHandlers() == 0;
// Now do the actual tests to see if we should return early.
// It is important here that we don't call dBodyIsEnabled on
// a static body because that crashes ODE.
bool case1 = neitherHasCollisionHandler &&
noGlobalCollisionHandlers && solid0Static && solid1Static;
//bool case2= o0BodyID == o1BodyID;
bool case3 = bothHaveBodies && !dBodyIsEnabled(o0BodyID)
&& !dBodyIsEnabled(o1BodyID);
bool case4 = commonJoint &&
commonJoint->getType() == FIXED_JOINT;
bool case5 = commonJoint
&& !commonJoint->areContactsEnabled();
bool case6 = solid0Static && 0 != o1BodyID
&& !dBodyIsEnabled(o1BodyID)
&& ignoreStaticSleepingContacts
&& neitherHasCollisionHandler && noGlobalCollisionHandlers;
bool case7 = solid1Static && 0 != o0BodyID
&& !dBodyIsEnabled(o0BodyID)
&& ignoreStaticSleepingContacts
&& neitherHasCollisionHandler && noGlobalCollisionHandlers;
bool case8 = !makeContacts && neitherHasCollisionHandler
&& noGlobalCollisionHandlers;
if (case1 || case3 || case4 || case5 || case6 || case7 || case8)
{
return;
}
// Now actually test for collision between the two geoms.
// This is one of the more expensive operations.
dWorldID theWorldID = sim->internal_getWorldID();
dJointGroupID theJointGroupID =
sim->internal_getJointGroupID();
dContactGeom contactArray[globals::maxMaxContacts];
int numContacts = dCollide(o0, o1, sim->getMaxContacts(),
contactArray, sizeof(dContactGeom));
// If the two objects didn't make any contacts, they weren't
// touching, so just return.
if (0 == numContacts)
{
return ;
}
// If at least one of the Solids has a CollisionEventHandler,
// send it a CollisionEvent.
if (handler0 || handler1 || !noGlobalCollisionHandlers)
{
// Call the CollisionEventHandlers. Note that we only
// use one contact point per collision: just the first one
// in the contact array. The order of the Solids
// passed to the event handlers is important: the first
// one should be the one whose event handler is
// getting called.
CollisionEvent e;
e.thisSolid = solid0;
e.otherSolid = solid1;
e.pos[0] = (real) contactArray[0].pos[0];
e.pos[1] = (real) contactArray[0].pos[1];
e.pos[2] = (real) contactArray[0].pos[2];
e.normal[0] = (real) contactArray[0].normal[0];
e.normal[1] = (real) contactArray[0].normal[1];
e.normal[2] = (real) contactArray[0].normal[2];
e.depth = (real) contactArray[0].depth;
if (handler0)
{
handler0->internal_pushCollisionEvent(e);
}
if (handler1)
{
// For the other Solid's CollisionEventHandler, we need
// to invert the normal and swap the Solid pointers.
e.normal *= -1;
e.thisSolid = solid1;
e.otherSolid = solid0;
handler1->internal_pushCollisionEvent(e);
}
sim->internal_recordCollision(e);
}
// Old version...
//// Early check to save some time.
//if (solid0->getCollisionEventHandler()
// || solid1->getCollisionEventHandler())
//{
// // Call the event handlers. Note: we only use one
// // contact point per collision; just use the first one
// // in the contact array. The order of the Solids
// // passed to the event handlers is important: the first
// // one should be the one whose event handler is
// // getting called.
// CollisionEvent e;
// e.solid0 = solid0;
// e.solid1 = solid1;
// e.pos[0] = contactArray[0].pos[0];
// e.pos[1] = contactArray[0].pos[1];
// e.pos[2] = contactArray[0].pos[2];
// e.normal[0] = contactArray[0].normal[0];
// e.normal[1] = contactArray[0].normal[1];
// e.normal[2] = contactArray[0].normal[2];
// e.depth = contactArray[0].depth;
// EventHandler* eventHandler =
// solid0->getCollisionEventHandler();
// if (eventHandler)
// {
// generateContacts0 =
// eventHandler->handleCollisionEvent(e);
// }
// e.normal *= -1; // Invert normal.
// e.solid0 = solid1; // Swap solid pointers.
// e.solid1 = solid0;
// eventHandler = solid1->getCollisionEventHandler();
// if (eventHandler)
// {
// generateContacts1 =
// eventHandler->handleCollisionEvent(e);
// }
//}
if (makeContacts)
{
// Invalidate the "freely-spinning" parameters.
((ODESolid*)solid0)->internal_setFreelySpinning(false);
((ODESolid*)solid1)->internal_setFreelySpinning(false);
for (int i = 0; i < numContacts; ++i)
{
const Material* m0 = &(shape0->material);
const Material* m1 = &(shape1->material);
dContact tempContact;
tempContact.surface.mode = dContactBounce |
dContactSoftERP; // | dContactSoftCFM;
// Average the hardness of the two materials.
assert(m0->hardness >= 0 && m0->hardness <= 1
&& m1->hardness >= 0 && m1->hardness <= 1);
real hardness = (m0->hardness + m1->hardness) *
(real) 0.5;
// Convert hardness to ERP. As hardness goes from
// 0.0 to 1.0, ERP goes from min to max.
tempContact.surface.soft_erp = hardness *
(defaults::ode::maxERP - defaults::ode::minERP) +
defaults::ode::minERP;
// Don't use contact CFM anymore. Just let it use
// the global value set in the ODE world.
//tempContact.surface.soft_cfm =
// defaults::ode::minCFM;
// As friction goes from 0.0 to 1.0, mu goes from 0.0
// to max, though it is set to dInfinity when
// friction == 1.0.
assert(m0->friction >= 0 && m0->friction <= 1
&& m1->friction >= 0 && m1->friction <= 1);
if (1.0 == m0->friction && 1.0 == m1->friction)
{
tempContact.surface.mu = dInfinity;
}
else
{
tempContact.surface.mu =
sqrt(m0->friction * m1->friction) *
defaults::ode::maxFriction;
}
// Average the bounciness of the two materials.
assert(m0->bounciness >= 0 && m0->bounciness <= 1
&& m1->bounciness >= 0 && m1->bounciness <= 1);
real bounciness = (m0->bounciness + m1->bounciness) *
(real) 0.5;
// ODE's bounce parameter, a.k.a. restitution.
tempContact.surface.bounce = bounciness;
// ODE's bounce_vel parameter is a threshold:
// the relative velocity of the two objects must be
// greater than this for bouncing to occur at all.
tempContact.surface.bounce_vel =
defaults::bounceThreshold;
// Old way to calculate bounce threshold; threshold
// was scaled by the collision bounciness, but this
// makes all objects with non-zero bounciness
// always bounce. This is undesirable because it
// takes a while for bouncing to totally diminish.
//tempContact.surface.bounce_vel =
// defaults::ode::maxBounceVel - bounciness *
// defaults::ode::maxBounceVel;
tempContact.geom = contactArray[i];
dJointID contactJoint = dJointCreateContact(
theWorldID, theJointGroupID, &tempContact);
// Note: the following line of code replaces the
// rest of this function which is commented out.
// TODO: test this and make sure the mass ratio
// issues are unimportant and that we never need
// "one-sided" contacts between two Solids.
dJointAttach(contactJoint, o0BodyID, o1BodyID);
//if (!bothHaveBodies)
//{
// // at least one object is static, so just handle
// // things like normal
// dJointAttach(contactJoint, o0BodyID, o1BodyID);
//}
//else
//{
// // TODO: We probably need to remove the following chunk of
// // code. The first case is obsolete since now both sides
// // always get contacts, if at all. (There isn't really a
// // good reason to have one side use contacts but not the
// // other; the side not wanting contacts would appear to be
// // static, so just make it static in the first place. On
// // the other hand, this may end up being desirable if
// // an object should be moved by some objects but not
// // others, and the "others" *do* collid with the first
// // object... but this situation may never come up. The
// // second case, using mass ratios to determine whether two
// // objects should collide, might not be an issue. Mass
// // ratios might only be a problem when the two objects are
// // constantly connected with a Joint.
// // Handle one-sided contacts for two cases: 1) only one of
// // the above event handlers actually wants contacts generated,
// // 2) if the mass ratio is above some threshold, treat it as
// // a one-sided collision solution: treat the more massive
// // object as static, calculate the collision like normal
// // (with the massive one being static), then also add the
// // massive object's velocity to the smaller one (velocity
// // calculated at the point of collision).
// // calculate mass ratio (use mass1 / mass2); if
// // the ratio is too high, mass1 is too large; if
// // the ratio is too low, mass2 is too large
// real massRatio = 0;
// dMass mass0, mass1;
// dBodyGetMass(o0BodyID, &mass0);
// dBodyGetMass(o1BodyID, &mass1);
// massRatio = mass0.mass / mass1.mass;
// // here we handle all the different collision
// // cases: one solid or the other or both may want
// // contacts generated; also, the mass ratio may
// // be outside the acceptable range
// if (true == generateContacts0 && true ==
// generateContacts1)
// {
// // both want contacts, neither wants to be
// // static
// if (massRatio > defaults::ode::maxMassRatio)
// {
// // ratio is too high - mass0 is too large,
// // treat solid0 as static
// dBodyEnable(o1BodyID);
// dJointAttach(contactJoint, 0, o1BodyID);
// }
// else if (massRatio <
// defaults::ode::minMassRatio)
// {
// // ratio is too low - mass1 is too large,
// // treat solid1 as static
// dBodyEnable(o0BodyID);
// dJointAttach(contactJoint, o0BodyID, 0);
// }
// else
// {
// //ratio is good - no static objects
// dJointAttach(contactJoint, o0BodyID,
// o1BodyID);
// }
// }
// else if (true == generateContacts0)
// {
// // solid0 wants contacts, solid1 wants to be
// // static
// if (massRatio > defaults::ode::maxMassRatio)
// {
// // ratio is too high - mass0 is too large,
// // treat solid0 and solid1 as static
// // i.e. don't generate a contact joint
// }
// else
// {
// // this block handles two cases which have
// // the same result:
// // 1. ratio is too low - mass1 is too
// // large, treat solid1 as static
// // 2. ratio is good - treat solid1 as
// // static
// dBodyEnable(o0BodyID);
// dJointAttach(contactJoint, o0BodyID, 0);
// }
// }
// else //generateContacts1 must be true
// {
// // solid1 wants contacts, solid0 wants to be
// // static
// if (massRatio < defaults::ode::minMassRatio)
// {
// // ratio is too low - mass1 is too large,
// // treat solid0 and solid1 as static
// // i.e. don't generate a contact joint
// }
// else
// {
// // this block handles two cases which have
// // the same result:
// // 1. ratio is too high - mass0 is too
// // large, treat solid0 as static
// // 2. ratio is good - treat solid0 as
// // static
// dBodyEnable(o1BodyID);
// dJointAttach(contactJoint, 0, o1BodyID);
// }
// }
//}
}
}
}
}
COLLADASW::Joint KDLColladaLibraryJointsExporter::makeColladaSWJoint(COLLADASW::StreamWriter* streamWriter, Joint& kdlJoint, string uniqueId)
{
string jointName = kdlJoint.getName();
if (jointName == kdlDefaultJointName)
jointName = uniqueId;
Joint::JointType jointType = kdlJoint.getType();
Vector jointAxis = kdlJoint.JointAxis();
COLLADAFW::JointPrimitive::Type type;
switch (jointType)
{
case Joint::RotAxis :
{
type = COLLADAFW::JointPrimitive::REVOLUTE;
break;
}
case Joint::RotX :
{
type = COLLADAFW::JointPrimitive::REVOLUTE;
break;
}
case Joint::RotY :
{
type = COLLADAFW::JointPrimitive::REVOLUTE;
break;
}
case Joint::RotZ :
{
type = COLLADAFW::JointPrimitive::REVOLUTE;
break;
}
case Joint::TransAxis :
{
type = COLLADAFW::JointPrimitive::PRISMATIC;
break;
}
case Joint::TransX :
{
type = COLLADAFW::JointPrimitive::PRISMATIC;
break;
}
case Joint::TransY :
{
type = COLLADAFW::JointPrimitive::PRISMATIC;
break;
}
case Joint::TransZ :
{
type = COLLADAFW::JointPrimitive::PRISMATIC;
break;
}
default :
LOG(ERROR) << "Unknown joint type: " << kdlJoint.getTypeName() << ", changing to revolute type with no axis.";
type = COLLADAFW::JointPrimitive::REVOLUTE;
}
COLLADABU::Math::Vector3 axis(jointAxis.x(), jointAxis.y(), jointAxis.z());
COLLADAFW::JointPrimitive jointPrimitive(COLLADAFW::UniqueId(uniqueId), type);
jointPrimitive.setAxis(axis);
// jointPrimitive.setHardLimitMax(10);
// jointPrimitive.setHardLimitMin(-10);
COLLADASW::Joint colladaJoint(streamWriter, jointPrimitive, uniqueId, jointName); //TODO make sure that ID is unique
// kdlJoint.name = uniqueId;
LOG(INFO) << "Joint id: " << colladaJoint.getJointId();
LOG(INFO) << "Joint name: " << colladaJoint.getJointName();
return colladaJoint;
}