Exemplo n.º 1
0
  void timeStepping() override
  {
    mController->clearForces();
    
    // Lesson 3
    mController->addSPDForces();

    // Lesson 4
    mController->addAnkleStrategyForces();
    
    // Lesson 6
    mController->setWheelCommands();
    
    // Apply body forces based on user input, and color the body shape red
    if(mForceCountDown > 0)
    {
      BodyNode* bn = mWorld->getSkeleton("biped")->getBodyNode("h_abdomen");
      const ShapePtr& shape = bn->getVisualizationShape(0);
      shape->setColor(dart::Color::Red());
      
      if(mPositiveSign)
        bn->addExtForce(default_force * Eigen::Vector3d::UnitX(),
                        bn->getCOM(), false, false);
      else
        bn->addExtForce(-default_force*Eigen::Vector3d::UnitX(),
                        bn->getCOM(), false, false);
      
      --mForceCountDown;
    }
    
    // Step the simulation forward
    SimWindow::timeStepping();
  }
Exemplo n.º 2
0
void setAllColors(const SkeletonPtr& object, const Eigen::Vector3d& color)
{
  // Set the color of all the shapes in the object
  for(size_t i=0; i < object->getNumBodyNodes(); ++i)
  {
    BodyNode* bn = object->getBodyNode(i);
    for(size_t j=0; j < bn->getNumVisualizationShapes(); ++j)
      bn->getVisualizationShape(j)->setColor(color);
  }
}
Exemplo n.º 3
0
SkeletonPtr createHubo()
{
  dart::utils::DartLoader loader;
  loader.addPackageDirectory("drchubo", DART_DATA_PATH"/urdf/drchubo");
  SkeletonPtr hubo =
      loader.parseSkeleton(DART_DATA_PATH"/urdf/drchubo/drchubo.urdf");

  for(size_t i = 0; i < hubo->getNumBodyNodes(); ++i)
  {
    BodyNode* bn = hubo->getBodyNode(i);
    if(bn->getName().substr(0, 7) == "Body_LF"
       || bn->getName().substr(0, 7) == "Body_RF"
       || bn->getName().substr(0, 7) == "Body_NK")
    {
      bn->remove();
      --i;
    }
  }

  hubo->setPosition(5, 0.97);

  for(size_t i=1; i < hubo->getNumJoints(); ++i)
  {
    hubo->getJoint(i)->setActuatorType(Joint::VELOCITY);
  }

  for(size_t i=0; i < hubo->getNumBodyNodes(); ++i)
  {
    BodyNode* bn = hubo->getBodyNode(i);
    for(size_t j=0; j < bn->getNumVisualizationShapes(); ++j)
    {
      const ShapePtr& shape = bn->getVisualizationShape(j);
      shape->setColor(Eigen::Vector3d(0.2, 0.2, 0.2));

      if(MeshShapePtr mesh = std::dynamic_pointer_cast<MeshShape>(shape))
        mesh->setColorMode(MeshShape::SHAPE_COLOR);
    }
  }

  hubo->setName("drchubo");
  return hubo;
}