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();
}
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);
}
}
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;
}
