예제 #1
0
bool PhysicsJointFixed::init(PhysicsBody* a, PhysicsBody* b, const Vec2& anchr)
{
    do
    {
        CC_BREAK_IF(!PhysicsJoint::init(a, b));
        
        getBodyNode(a)->setPosition(anchr);
        getBodyNode(b)->setPosition(anchr);
        
        // add a pivot joint to fixed two body together
        auto constraint = cpPivotJointNew(a->getCPBody(), b->getCPBody(),
                                        PhysicsHelper::point2cpv(anchr));
        CC_BREAK_IF(constraint == nullptr);
        _cpConstraints.push_back(constraint);
        
        // add a gear joint to make two body have the same rotation.
        constraint = cpGearJointNew(a->getCPBody(), b->getCPBody(), 0, 1);
        CC_BREAK_IF(constraint == nullptr);
        _cpConstraints.push_back(constraint);
        
        setCollisionEnable(false);
        
        return true;
    } while (false);
    
    return false;
}
bool PhysicsJointFixed::init(PhysicsBody* a, PhysicsBody* b, const Point& anchr)
{
    do
    {
        CC_BREAK_IF(!PhysicsJoint::init(a, b));
        
        getBodyNode(a)->setPosition(anchr);
        getBodyNode(b)->setPosition(anchr);
        
        // add a pivot joint to fixed two body together
        cpConstraint* joint = cpPivotJointNew(getBodyInfo(a)->getBody(), getBodyInfo(b)->getBody(),
                                        PhysicsHelper::point2cpv(anchr));
        CC_BREAK_IF(joint == nullptr);
        m_pInfo->add(joint);
        
        // add a gear joint to make two body have the same rotation.
        joint = cpGearJointNew(getBodyInfo(a)->getBody(), getBodyInfo(b)->getBody(), 0, 1);
        CC_BREAK_IF(joint == nullptr);
        m_pInfo->add(joint);
        
        setCollisionEnable(false);
        
        return true;
    } while (false);
    
    return false;
}
예제 #3
0
파일: Skeleton.cpp 프로젝트: hsu/dart
int Skeleton::getBodyNodeIndex(const std::string& _name) const
{
    const int nNodes = getNumBodyNodes();

    for(int i = 0; i < nNodes; i++)
    {
        BodyNode* node = getBodyNode(i);

        if (_name == node->getName())
            return i;
    }

    return -1;
}
예제 #4
0
파일: Skeleton.cpp 프로젝트: hsu/dart
Eigen::Vector3d Skeleton::getWorldCOM()
{
    Eigen::Vector3d com(0, 0, 0);

    assert(mTotalMass != 0);
    const int nNodes = getNumBodyNodes();

    for(int i = 0; i < nNodes; i++)
    {
        BodyNode* bodyNode = getBodyNode(i);
        com += (bodyNode->getMass() * bodyNode->getWorldCOM());
    }

    return com / mTotalMass;
}
예제 #5
0
//==============================================================================
void ReferentialSkeleton::clearCollidingBodies()
{
  for (auto i = 0u; i < getNumBodyNodes(); ++i)
  {
    auto bodyNode = getBodyNode(i);
DART_SUPPRESS_DEPRECATED_BEGIN
    bodyNode->setColliding(false);
DART_SUPPRESS_DEPRECATED_END

    auto softBodyNode = bodyNode->asSoftBodyNode();
    if (softBodyNode)
    {
      auto& pointMasses = softBodyNode->getPointMasses();

      for (auto pointMass : pointMasses)
        pointMass->setColliding(false);
    }
  }
}
예제 #6
0
파일: Skeleton.cpp 프로젝트: hsu/dart
void Skeleton::computeEquationsOfMotionID(
        const Eigen::Vector3d& _gravity)
{
    int n = getDOF();

    // skip immobile objects in forward simulation
    if (getImmobileState() == true || n == 0)
    {
        return;
    }

    // Save current tau
    Eigen::VectorXd tau_old = get_tau();

    // Set ddq as zero
    set_ddq(Eigen::VectorXd::Zero(n));

    // M(q) * ddq + b(q,dq) = tau
    computeInverseDynamicsLinear(_gravity, true);
    mCg = get_tau();

    // Calcualtion mass matrix, M
    mM = Eigen::MatrixXd::Zero(n,n);
    for (int i = 0; i < getNumBodyNodes(); i++)
    {
        BodyNode *nodei = getBodyNode(i);
        nodei->updateMassMatrix();
        nodei->aggregateMass(mM);
    }

    // Inverse of mass matrix
    mMInv = mM.ldlt().solve(Eigen::MatrixXd::Identity(n,n));

    // Restore the torque
    set_tau(tau_old);

    // Evaluate external forces in generalized coordinate.
    updateExternalForces();

    // Update damping forces
    updateDampingForces();
}
예제 #7
0
파일: Skeleton.cpp 프로젝트: hsu/dart
void Skeleton::initDynamics()
{
    initKinematics();

    int DOF = getDOF();

    mM    = Eigen::MatrixXd::Zero(DOF, DOF);
    mMInv = Eigen::MatrixXd::Zero(DOF, DOF);
    mC    = Eigen::MatrixXd::Zero(DOF, DOF);
    mCvec = Eigen::VectorXd::Zero(DOF);
    mG    = Eigen::VectorXd::Zero(DOF);
    mCg   = Eigen::VectorXd::Zero(DOF);
    set_tau(Eigen::VectorXd::Zero(DOF));
    mFext = Eigen::VectorXd::Zero(DOF);
    mFc   = Eigen::VectorXd::Zero(DOF);
    mDampingForce = Eigen::VectorXd::Zero(DOF);

    // calculate mass
    // init the dependsOnDof stucture for each bodylink
    mTotalMass = 0.0;
    for(int i = 0; i < getNumBodyNodes(); i++)
        mTotalMass += getBodyNode(i)->getMass();
}
예제 #8
0
//==============================================================================
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";
        }
      }
    }
  }

}