Matrix SkeletonBlendedGeometry::getBindTransformationDiff(UInt32 index) const
{
Matrix Result(getInternalJointInvBindTransformations(index));
Result.multLeft(dynamic_cast<Node*>(getJoint(index)->getParents()[0])->getToWorld());
return Result;
}
void SSimRobotEntity::addJoint(SSimJoint *joint)
{
// if a case that does not have geommetry
if (!joint->has_geom) {
// create dummy body
joint->robotParts.objParts.body = dBodyCreate(m_world);
dBodyDisable(joint->robotParts.objParts.body);
/*
dGeomID geom = dCreateSphere(m_space, 0.1);
dGeomSetBody(geom, joint->robotParts.objParts.body);
dGeomDisable(geom);
dMass mass;
dMassSetZero(&mass);
dMassSetSphereTotal(&mass, 0.0000001, 0.0000001);
dBodySetMass(joint->robotParts.objParts.body, &mass);
*/
//LOG_MSG(("dummy body %d",joint->robotParts.objParts.body));
}
// setting mass
else {
double mass = joint->robotParts.objParts.mass;
this->setMass(&(joint->robotParts.objParts), mass);
// add robot parts which have geometry to member variables
m_allParts.push_back(joint->robotParts);
}
// add joint to member variables
m_allJoints.push_back(joint);
// refer the joint ID and body ID
dJointID myJoint = joint->joint;
dBodyID cbody = joint->robotParts.objParts.body;
if (joint->isRoot) {
// connect with root body
//dJointAttach(myJoint, cbody, m_rootBody); //TODO!
return;
}
// refer the name of parent joint
std::string parent = joint->parent_joint;
// refer the parent joint structure from the joint name
//SSimJoint pjoint;
SSimJoint *pjoint = getJoint(parent);
//LOG_MSG(("parent %s %s",parent.c_str(), pjoint.name.c_str()));
// connect with parent body
dBodyID pbody = pjoint->robotParts.objParts.body;
//dJointAttach(myJoint, pbody, cbody);
dJointAttach(myJoint, cbody, pbody);
int type = dJointGetType(myJoint);
if (type == dJointTypeHinge) {
dJointSetHingeParam(myJoint, dParamLoStop, -2.0*M_PI);
dJointSetHingeParam(myJoint, dParamHiStop, 2.0*M_PI);
}
LOG_MSG(("joint(%d) attach body(%d, %d)",myJoint, cbody, pbody));
//dGeomID geom1 = pjoint->robotParts.objParts.mass;
//dGeomID geom2 = joint->robotParts.objParts.mass;
//LOG_MSG(("geom (%d, %d)", geom1, geom2));
/*
// if geommetry exists
if (joint.has_geom) {
/*
////// search the parent part to be connected
// refer the parent joint
std::string parent = joint.parent_joint;
while (1) {
// get joint structure from joint name
SSimJoint pjoint = getJoint(parent);
// check whether geometry to be connected exist?
if (pjoint.has_geom) {
// refer a body to be connected
dBodyID pbody = pjoint.robotParts.objParts.body;
dJointAttach(myJoint, cbody, pbody);
break;
}
// there is no parent any more
if (pjoint.isRoot) break;
else{
// Search the parents in addition
parent = pjoint.parent_name;
}
}
*/
}
bool setJointDefAnchor(b2JointDef* d, Box2D* rdPtr)
{
b2Body* b = d->body1;
b2Body* b2 = d->body2;
switch(d->type)
{
case e_revoluteJoint:
{
b2RevoluteJointDef* def = (b2RevoluteJointDef*)d;
switch(def->local1)
{
case 0:
def->localAnchor1 = b->GetLocalPoint(def->anchor1);
break;
case 2:
def->localAnchor1 = b->GetLocalPoint(b2->GetWorldPoint(def->anchor1));
break;
default:
def->localAnchor1 = def->anchor1;
break;
}
switch(def->local2)
{
case 0:
def->localAnchor2 = b2->GetLocalPoint(def->anchor2);
break;
case 2:
def->localAnchor2 = b2->GetLocalPoint(b->GetWorldPoint(def->anchor2));
break;
default:
def->localAnchor2 = def->anchor2;
break;
}
}
break;
case e_prismaticJoint:
{
b2PrismaticJointDef* def = (b2PrismaticJointDef*)d;
switch(def->local1)
{
case 0:
def->localAnchor1 = b->GetLocalPoint(def->anchor1);
break;
case 2:
def->localAnchor1 = b->GetLocalPoint(b2->GetWorldPoint(def->anchor1));
break;
default:
def->localAnchor1 = def->anchor1;
break;
}
switch(def->local2)
{
case 0:
def->localAnchor2 = b2->GetLocalPoint(def->anchor2);
break;
case 2:
def->localAnchor2 = b2->GetLocalPoint(b->GetWorldPoint(def->anchor2));
break;
default:
def->localAnchor2 = def->anchor2;
break;
}
switch(def->alocal)
{
case 1:
def->localAxis1.Set(cos(def->angle-b->GetAngle()),sin(def->angle-b->GetAngle()));
break;
case 2:
def->localAxis1.Set(cos(def->angle-b2->GetAngle()),sin(def->angle-b2->GetAngle()));
break;
default:
def->localAxis1.Set(cos(def->angle),sin(def->angle));
break;
}
}
break;
case e_lineJoint:
{
b2LineJointDef* def = (b2LineJointDef*)d;
switch(def->local1)
{
case 0:
def->localAnchor1 = b->GetLocalPoint(def->anchor1);
break;
case 2:
def->localAnchor1 = b->GetLocalPoint(b2->GetWorldPoint(def->anchor1));
break;
default:
def->localAnchor1 = def->anchor1;
break;
}
switch(def->local2)
{
case 0:
def->localAnchor2 = b2->GetLocalPoint(def->anchor2);
break;
case 2:
def->localAnchor2 = b2->GetLocalPoint(b->GetWorldPoint(def->anchor2));
break;
default:
def->localAnchor2 = def->anchor2;
break;
}
switch(def->alocal)
{
case 1:
def->localAxis1.Set(cos(def->angle-b->GetAngle()),sin(def->angle-b->GetAngle()));
break;
case 2:
def->localAxis1.Set(cos(def->angle-b2->GetAngle()),sin(def->angle-b2->GetAngle()));
break;
default:
def->localAxis1.Set(cos(def->angle),sin(def->angle));
break;
}
}
break;
case e_distanceJoint:
{
b2DistanceJointDef* def = (b2DistanceJointDef*)d;
switch(def->local1)
{
case 0:
def->localAnchor1 = b->GetLocalPoint(def->anchor1);
break;
case 2:
def->localAnchor1 = b->GetLocalPoint(b2->GetWorldPoint(def->anchor1));
break;
default:
def->localAnchor1 = def->anchor1;
break;
}
switch(def->local2)
{
case 0:
def->localAnchor2 = b2->GetLocalPoint(def->anchor2);
break;
case 2:
def->localAnchor2 = b2->GetLocalPoint(b->GetWorldPoint(def->anchor2));
break;
default:
def->localAnchor2 = def->anchor2;
break;
}
switch(def->abslen)
{
case 0:
def->length = def->alength + (b->GetWorldPoint(def->localAnchor1) - b2->GetWorldPoint(def->localAnchor2)).Length();
break;
default:
def->length = def->alength;
break;
}
}
break;
case e_mouseJoint:
{
b2MouseJointDef* def = (b2MouseJointDef*)d;
switch(def->local)
{
case 1:
def->target = b2->GetWorldPoint(def->anchor);
break;
default:
def->target = def->anchor;
break;
}
}
break;
case e_gearJoint:
{
b2GearJointDef* def = (b2GearJointDef*)d;
switch(def->local1)
{
case 1:
{
b2JointEdge* je = b->GetJointList();
for(int i = 0; i < def->joint1n; i++)
{
if(!je) return false;
je = je->next;
}
if(!je) return false;
def->joint1 = je->joint;
}
break;
default:
def->joint1 = getJoint(def->joint1n,rdPtr);
if(!def->joint1) return false;
break;
}
switch(def->local2)
{
case 1:
{
b2JointEdge* je = b2->GetJointList();
for(int i = 0; i < def->joint2n; i++)
{
if(!je) return false;
je = je->next;
}
if(!je) return false;
def->joint2 = je->joint;
}
break;
default:
def->joint2 = getJoint(def->joint2n,rdPtr);
if(!def->joint2) return false;
break;
}
}
break;
case e_pulleyJoint:
{
b2PulleyJointDef* def = (b2PulleyJointDef*)d;
switch(def->local1)
{
case 0:
def->localAnchor1 = b->GetLocalPoint(def->anchor1);
break;
case 2:
def->localAnchor1 = b->GetLocalPoint(b2->GetWorldPoint(def->anchor1));
break;
default:
def->localAnchor1 = def->anchor1;
break;
}
switch(def->local2)
{
case 0:
def->localAnchor2 = b2->GetLocalPoint(def->anchor2);
break;
case 2:
def->localAnchor2 = b2->GetLocalPoint(b->GetWorldPoint(def->anchor2));
break;
default:
def->localAnchor2 = def->anchor2;
break;
}
switch(def->glocal1)
{
case 1:
def->groundAnchor1 = b->GetWorldPoint(def->ganchor1);
break;
case 2:
def->groundAnchor1 = b2->GetWorldPoint(def->ganchor1);
break;
default:
def->groundAnchor1 = def->ganchor1;
break;
}
switch(def->glocal2)
{
case 1:
def->groundAnchor2 = b2->GetWorldPoint(def->ganchor2);
break;
case 2:
def->groundAnchor2 = b->GetWorldPoint(def->ganchor2);
break;
default:
def->groundAnchor2 = def->ganchor2;
break;
}
switch(def->mlocal1)
{
case 0:
def->maxLength1 = def->mlength1 + (b->GetWorldPoint(def->localAnchor1) - def->groundAnchor1).Length();
break;
default:
def->maxLength1 = def->mlength1;
break;
}
switch(def->mlocal2)
{
case 0:
def->maxLength2 = def->mlength2 + (b2->GetWorldPoint(def->localAnchor2) - def->groundAnchor2).Length();
break;
default:
def->maxLength2 = def->mlength2;
break;
}
}
break;
}
return true;
}
void Robot::update(const LinkUpdater& updater)
{
M_NameToLink::iterator link_it = links_.begin();
M_NameToLink::iterator link_end = links_.end();
for ( ; link_it != link_end; ++link_it )
{
RobotLink* link = link_it->second;
link->setToNormalMaterial();
Ogre::Vector3 visual_position, collision_position;
Ogre::Quaternion visual_orientation, collision_orientation;
if( updater.getLinkTransforms( link->getName(),
visual_position, visual_orientation,
collision_position, collision_orientation
))
{
// Check if visual_orientation, visual_position, collision_orientation, and collision_position are NaN.
if (visual_orientation.isNaN())
{
ROS_ERROR_THROTTLE(
1.0,
"visual orientation of %s contains NaNs. Skipping render as long as the orientation is invalid.",
link->getName().c_str()
);
continue;
}
if (visual_position.isNaN())
{
ROS_ERROR_THROTTLE(
1.0,
"visual position of %s contains NaNs. Skipping render as long as the position is invalid.",
link->getName().c_str()
);
continue;
}
if (collision_orientation.isNaN())
{
ROS_ERROR_THROTTLE(
1.0,
"collision orientation of %s contains NaNs. Skipping render as long as the orientation is invalid.",
link->getName().c_str()
);
continue;
}
if (collision_position.isNaN())
{
ROS_ERROR_THROTTLE(
1.0,
"collision position of %s contains NaNs. Skipping render as long as the position is invalid.",
link->getName().c_str()
);
continue;
}
link->setTransforms( visual_position, visual_orientation, collision_position, collision_orientation );
std::vector<std::string>::const_iterator joint_it = link->getChildJointNames().begin();
std::vector<std::string>::const_iterator joint_end = link->getChildJointNames().end();
for ( ; joint_it != joint_end ; ++joint_it )
{
RobotJoint *joint = getJoint(*joint_it);
if (joint)
{
joint->setTransforms(visual_position, visual_orientation);
}
}
}
else
{
link->setToErrorMaterial();
}
}
}
//==============================================================================
TEST(Issue1184, Accuracy)
{
struct ShapeInfo
{
dart::dynamics::ShapePtr shape;
double offset;
};
std::function<ShapeInfo()> makePlaneGround =
[]()
{
return ShapeInfo{
std::make_shared<dart::dynamics::PlaneShape>(
Eigen::Vector3d::UnitZ(), 0.0),
0.0};
};
std::function<ShapeInfo()> makeBoxGround =
[]()
{
const double thickness = 0.1;
return ShapeInfo{
std::make_shared<dart::dynamics::BoxShape>(
Eigen::Vector3d(100.0, 100.0, thickness)),
-thickness/2.0};
};
std::function<dart::dynamics::ShapePtr(double)> makeBoxObject =
[](const double s) -> dart::dynamics::ShapePtr
{
return std::make_shared<dart::dynamics::BoxShape>(
Eigen::Vector3d::Constant(2*s));
};
std::function<dart::dynamics::ShapePtr(double)> makeSphereObject =
[](const double s) -> dart::dynamics::ShapePtr
{
return std::make_shared<dart::dynamics::SphereShape>(s);
};
#ifndef NDEBUG
const auto groundInfoFunctions = {makePlaneGround};
const auto objectShapeFunctions = {makeSphereObject};
const auto halfsizes = {10.0};
const auto fallingModes = {true};
const double dropHeight = 0.1;
const double tolerance = 1e-3;
#else
const auto groundInfoFunctions = {makePlaneGround, makeBoxGround};
const auto objectShapeFunctions = {makeBoxObject, makeSphereObject};
const auto halfsizes = {0.25, 1.0, 5.0, 10.0, 20.0};
const auto fallingModes = {true, false};
const double dropHeight = 1.0;
const double tolerance = 1e-3;
#endif
for(const auto& groundInfoFunction : groundInfoFunctions)
{
for(const auto& objectShapeFunction : objectShapeFunctions)
{
for(const double halfsize : halfsizes)
{
for(const bool falling : fallingModes)
{
auto world = dart::simulation::World::create("test");
world->getConstraintSolver()->setCollisionDetector(
dart::collision::BulletCollisionDetector::create());
Eigen::Isometry3d tf_object = Eigen::Isometry3d::Identity();
const double initialHeight = falling? dropHeight+halfsize : halfsize;
tf_object.translate(initialHeight*Eigen::Vector3d::UnitZ());
auto object = dart::dynamics::Skeleton::create("ball");
object->createJointAndBodyNodePair<dart::dynamics::FreeJoint>()
.first->setTransform(tf_object);
const auto objectShape = objectShapeFunction(halfsize);
object->getBodyNode(0)->createShapeNodeWith<
dart::dynamics::VisualAspect,
dart::dynamics::CollisionAspect>(objectShape);
world->addSkeleton(object);
const ShapeInfo groundInfo = groundInfoFunction();
auto ground = dart::dynamics::Skeleton::create("ground");
ground->createJointAndBodyNodePair<dart::dynamics::WeldJoint>()
.second->createShapeNodeWith<
dart::dynamics::VisualAspect,
dart::dynamics::CollisionAspect>(groundInfo.shape);
Eigen::Isometry3d tf_ground = Eigen::Isometry3d::Identity();
tf_ground.translate(groundInfo.offset*Eigen::Vector3d::UnitZ());
ground->getJoint(0)->setTransformFromParentBodyNode(tf_ground);
world->addSkeleton(ground);
// time until the object will strike
const double t_strike = falling?
sqrt(-2.0*dropHeight/world->getGravity()[2]) : 0.0;
// give the object some time to settle
const double min_time = 0.5;
const double t_limit = 30.0*t_strike + min_time;
double lowestHeight = std::numeric_limits<double>::infinity();
double time = 0.0;
while(time < t_limit)
{
world->step();
const double currentHeight =
object->getBodyNode(0)->getTransform().translation()[2];
if(currentHeight < lowestHeight)
lowestHeight = currentHeight;
time = world->getTime();
}
// The simulation should have run for at least two seconds
ASSERT_LE(min_time, time);
EXPECT_GE(halfsize+tolerance, lowestHeight)
<< "object type: " << objectShape->getType()
<< "\nground type: " << groundInfo.shape->getType()
<< "\nfalling: " << falling << "\n";
const double finalHeight =
object->getBodyNode(0)->getTransform().translation()[2];
EXPECT_NEAR(halfsize, finalHeight, tolerance)
<< "object type: " << objectShape->getType()
<< "\nground type: " << groundInfo.shape->getType()
<< "\nfalling: " << falling << "\n";
}
}
}
}
}
void pJointBall::setProjectionMode(ProjectionMode mode)
{
NxSphericalJointDesc descr; NxSphericalJoint *joint = static_cast<NxSphericalJoint*>(getJoint()); if (!joint)return ; joint->saveToDesc(descr);
descr.projectionMode = (NxJointProjectionMode)mode;
joint->loadFromDesc(descr);
}
void pJointBall::setAnchor( const VxVector& anchor )
{
NxSphericalJointDesc descr;
NxSphericalJoint *joint = static_cast<NxSphericalJoint*>(getJoint()); if (!joint)return ; joint->saveToDesc(descr);
joint->setGlobalAnchor(pMath::getFrom(anchor));
}
void pJointBall::setProjectionDistance(float distance)
{
NxSphericalJointDesc descr; NxSphericalJoint *joint = static_cast<NxSphericalJoint*>(getJoint()); if (!joint)return ; joint->saveToDesc(descr);
descr.projectionDistance= distance;
joint->loadFromDesc(descr);
}
