OldConstraint* MyWindow::addTailConstraint() {
BodyNode *bd = mWorld->getSkeleton(0)->getBodyNode("link 10");
Eigen::Vector3d offset(0.0, -0.025, 0.0);
Eigen::Vector3d target = bd->getTransform() * offset;
// OldBallJointConstraint *bj = new OldBallJointConstraint(bd, offset, target);
// mWorld->getConstraintHandler()->addConstraint(bj);
// return bj;
return NULL;
}
void CollisionInterface::postProcess() {
mContacts.clear();
int numContacts = mCollisionChecker->getNumContacts();
mContacts.resize(numContacts);
for (int i = 0; i < numContacts; i++) {
mContacts[i].point = mCollisionChecker->getContact(i).point;
mContacts[i].normal = mCollisionChecker->getContact(i).normal;
mContacts[i].rb1 = mNodeMap[mCollisionChecker->getContact(i).bodyNode1];
mContacts[i].rb2 = mNodeMap[mCollisionChecker->getContact(i).bodyNode2];
if(mContacts[i].rb1 == NULL) {
BodyNode* bd = mCollisionChecker->getContact(i).bodyNode1;
Vector3d localPoint = bd->getTransform().inverse() * mContacts[i].point;
mContacts[i].pinataVelocity = bd->getWorldLinearVelocity(localPoint);
} else if(mContacts[i].rb2 == NULL) {
BodyNode* bd = mCollisionChecker->getContact(i).bodyNode2;
Vector3d localPoint = bd->getTransform().inverse() * mContacts[i].point;
mContacts[i].pinataVelocity = bd->getWorldLinearVelocity(localPoint);
} else {
mContacts[i].pinataVelocity.setZero();
}
}
}
//==============================================================================
TEST(FORWARD_KINEMATICS, YAW_ROLL)
{
// Checks forward kinematics for two DoF arm manipulators.
// NOTE: The following is the reference frame description of the world
// frame. The x-axis is into the page, z-axis is to the top of the
// page and the y-axis is to the left. At the zero angle, the links
// are parallel to the z-axis and face the +x-axis.
// Create the world
const double l1 = 1.5, l2 = 1.0;
SkeletonPtr robot = createTwoLinkRobot(Vector3d(0.3, 0.3, l1), DOF_YAW,
Vector3d(0.3, 0.3, l2), DOF_ROLL);
// Set the test cases with the joint values and the expected end-effector
// positions
const std::size_t numTests = 2;
double temp = sqrt(0.5*l2*l2);
Vector2d joints [numTests] = { Vector2d( constantsd::pi()/4.0, constantsd::pi()/2.0),
Vector2d(-constantsd::pi()/4.0, -constantsd::pi()/4.0) };
Vector3d expectedPos [numTests] = { Vector3d(temp, -temp, l1),
Vector3d(temp / sqrt(2.0),
temp / sqrt(2.0), l1+temp) };
// Check each case by setting the joint values and obtaining the end-effector
// position
for (std::size_t i = 0; i < numTests; i++)
{
robot->setPositions(Eigen::VectorXd(joints[i]));
BodyNode* bn = robot->getBodyNode("ee");
Vector3d actual = bn->getTransform().translation();
bool equality = equals(actual, expectedPos[i], 1e-3);
EXPECT_TRUE(equality);
if(!equality)
{
std::cout << "Joint values: " << joints[i].transpose() << std::endl;
std::cout << "Actual pos: " << actual.transpose() << std::endl;
std::cout << "Expected pos: " << expectedPos[i].transpose() << std::endl;
}
}
}
//==============================================================================
void ConstraintTest::SingleContactTest(const std::string& /*_fileName*/)
{
using namespace std;
using namespace Eigen;
using namespace dart::math;
using namespace dart::collision;
using namespace dart::constraint;
using namespace dart::dynamics;
using namespace dart::simulation;
using namespace dart::io;
//----------------------------------------------------------------------------
// Settings
//----------------------------------------------------------------------------
// Number of random state tests for each skeletons
#ifndef NDEBUG // Debug mode
// std::size_t testCount = 1;
#else
// std::size_t testCount = 1;
#endif
WorldPtr world = World::create();
EXPECT_TRUE(world != nullptr);
world->setGravity(Vector3d(0.0, -10.00, 0.0));
world->setTimeStep(0.001);
world->getConstraintSolver()->setCollisionDetector(
DARTCollisionDetector::create());
SkeletonPtr sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0));
BodyNode* sphere = sphereSkel->getBodyNode(0);
Joint* sphereJoint = sphere->getParentJoint();
sphereJoint->setVelocity(3, Random::uniform(-2.0, 2.0)); // x-axis
sphereJoint->setVelocity(5, Random::uniform(-2.0, 2.0)); // z-axis
world->addSkeleton(sphereSkel);
EXPECT_EQ(sphereSkel->getGravity(), world->getGravity());
assert(sphere);
SkeletonPtr boxSkel = createBox(Vector3d(1.0, 1.0, 1.0),
Vector3d(0.0, 1.0, 0.0));
BodyNode* box = boxSkel->getBodyNode(0);
Joint* boxJoint = box->getParentJoint();
boxJoint->setVelocity(3, Random::uniform(-2.0, 2.0)); // x-axis
boxJoint->setVelocity(5, Random::uniform(-2.0, 2.0)); // z-axis
// world->addSkeleton(boxSkel);
// EXPECT_EQ(boxSkel->getGravity(), world->getGravity());
// assert(box);
SkeletonPtr groundSkel = createGround(Vector3d(10000.0, 0.1, 10000.0),
Vector3d(0.0, -0.05, 0.0));
groundSkel->setMobile(false);
// BodyNode* ground = groundSkel->getBodyNode(0);
world->addSkeleton(groundSkel);
EXPECT_EQ(groundSkel->getGravity(), world->getGravity());
// assert(ground);
EXPECT_EQ((int)world->getNumSkeletons(), 2);
// Lower and upper bound of configuration for system
// double lb = -1.5 * constantsd::pi();
// double ub = 1.5 * constantsd::pi();
int maxSteps = 500;
for (int i = 0; i < maxSteps; ++i)
{
// Vector3d pos1 = sphere->getWorldTransform().translation();
// Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "vel1:" << vel1.transpose() << std::endl;
if (!world->checkCollision())
{
world->step();
continue;
}
// for (std::size_t j = 0; j < cd->getNumContacts(); ++j)
// {
// Contact contact = cd->getContact(j);
// Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
// Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "vel1:" << vel1.transpose() << std::endl;
// }
world->step();
const auto& result = world->getConstraintSolver()->getLastCollisionResult();
for (const auto& contact : result.getContacts())
{
Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
Vector3d vel1 = sphere->getLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "pos1[1]: " << pos1[1] << std::endl;
// std::cout << "pos1:" << pos1.transpose() << std::endl;
std::cout << "vel1:" << vel1.transpose() << ", pos1[1]: " << pos1[1] << std::endl;
// EXPECT_NEAR(pos1[0], 0.0, 1e-9);
// EXPECT_NEAR(pos1[1], -0.05, 1e-2);
// EXPECT_NEAR(pos1[2], 0.0, 1e-9);
// EXPECT_NEAR(vel1[0], 0.0, 1e-9);
// EXPECT_NEAR(vel1[1], 0.0, 1e-9);
// EXPECT_NEAR(vel1[2], 0.0, 1e-9);
// if (!equals(vel1, Vector3d(0.0, 0.0, 0.0)))
// std::cout << "vel1:" << vel1.transpose() << std::endl;
// EXPECT_EQ(vel1, Vector3d::Zero());
}
// std::cout << std::endl;
break;
}
}
//==============================================================================
void ConstraintTest::SingleContactTest(const std::string& _fileName)
{
using namespace std;
using namespace Eigen;
using namespace dart::math;
using namespace dart::collision;
using namespace dart::constraint;
using namespace dart::dynamics;
using namespace dart::simulation;
using namespace dart::utils;
//----------------------------------------------------------------------------
// Settings
//----------------------------------------------------------------------------
// Number of random state tests for each skeletons
#ifndef NDEBUG // Debug mode
int testCount = 1;
#else
int testCount = 1;
#endif
World* world = new World;
EXPECT_TRUE(world != NULL);
world->setGravity(Vector3d(0.0, -10.00, 0.0));
world->setTimeStep(0.001);
world->getConstraintSolver()->setCollisionDetector(
new DARTCollisionDetector());
Skeleton* sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0));
BodyNode* sphere = sphereSkel->getBodyNode(0);
Joint* sphereJoint = sphere->getParentJoint();
sphereJoint->setVelocity(3, random(-2.0, 2.0)); // x-axis
sphereJoint->setVelocity(5, random(-2.0, 2.0)); // z-axis
world->addSkeleton(sphereSkel);
EXPECT_EQ(sphereSkel->getGravity(), world->getGravity());
assert(sphere);
Skeleton* boxSkel = createBox(Vector3d(1.0, 1.0, 1.0),
Vector3d(0.0, 1.0, 0.0));
BodyNode* box = boxSkel->getBodyNode(0);
Joint* boxJoint = box->getParentJoint();
boxJoint->setVelocity(3, random(-2.0, 2.0)); // x-axis
boxJoint->setVelocity(5, random(-2.0, 2.0)); // z-axis
// world->addSkeleton(boxSkel);
// EXPECT_EQ(boxSkel->getGravity(), world->getGravity());
// assert(box);
Skeleton* groundSkel = createGround(Vector3d(10000.0, 0.1, 10000.0),
Vector3d(0.0, -0.05, 0.0));
groundSkel->setMobile(false);
BodyNode* ground = groundSkel->getBodyNode(0);
world->addSkeleton(groundSkel);
EXPECT_EQ(groundSkel->getGravity(), world->getGravity());
assert(ground);
EXPECT_EQ(world->getNumSkeletons(), 2);
ConstraintSolver* cs = world->getConstraintSolver();
CollisionDetector* cd = cs->getCollisionDetector();
// Lower and upper bound of configuration for system
double lb = -1.5 * DART_PI;
double ub = 1.5 * DART_PI;
int maxSteps = 500;
for (int i = 0; i < maxSteps; ++i)
{
// Vector3d pos1 = sphere->getWorldTransform().translation();
// Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "vel1:" << vel1.transpose() << std::endl;
cd->detectCollision(true, true);
if (cd->getNumContacts() == 0)
{
world->step();
continue;
}
for (int j = 0; j < cd->getNumContacts(); ++j)
{
Contact contact = cd->getContact(j);
Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "vel1:" << vel1.transpose() << std::endl;
}
world->step();
for (int j = 0; j < cd->getNumContacts(); ++j)
{
Contact contact = cd->getContact(j);
Vector3d pos1 = sphere->getTransform().inverse() * contact.point;
Vector3d vel1 = sphere->getWorldLinearVelocity(pos1);
// std::cout << "pos1:" << pos1.transpose() << std::endl;
// std::cout << "pos1[1]: " << pos1[1] << std::endl;
// std::cout << "pos1:" << pos1.transpose() << std::endl;
std::cout << "vel1:" << vel1.transpose() << ", pos1[1]: " << pos1[1] << std::endl;
// EXPECT_NEAR(pos1[0], 0.0, 1e-9);
// EXPECT_NEAR(pos1[1], -0.05, 1e-2);
// EXPECT_NEAR(pos1[2], 0.0, 1e-9);
// EXPECT_NEAR(vel1[0], 0.0, 1e-9);
// EXPECT_NEAR(vel1[1], 0.0, 1e-9);
// EXPECT_NEAR(vel1[2], 0.0, 1e-9);
// if (!equals(vel1, Vector3d(0.0, 0.0, 0.0)))
// std::cout << "vel1:" << vel1.transpose() << std::endl;
// EXPECT_EQ(vel1, Vector3d::Zero());
}
// std::cout << std::endl;
break;
}
delete world;
}
