std::pair<JointType*, dart::dynamics::BodyNode*> addBlock(
dart::dynamics::BodyNode* parent,
const Eigen::Isometry3d& relTf,
const dart::dynamics::ShapePtr& jointShape)
{
if (isSimulating())
{
std::cout << " -- Please pause simulation [using the Spacebar] before "
<< "attempting to add new bodies" << std::endl;
return std::make_pair(nullptr, nullptr);
}
dart::dynamics::SkeletonPtr skel;
if (parent)
{
skel = parent->getSkeleton();
}
else
{
skel = dart::dynamics::Skeleton::create(
"toy_#" + std::to_string(getWorld()->getNumSkeletons() + 1));
getWorld()->addSkeleton(skel);
}
auto pair = skel->createJointAndBodyNodePair<JointType>(parent);
JointType* joint = pair.first;
dart::dynamics::BodyNode* bn = pair.second;
bn->setName("block_#" + std::to_string(skel->getNumBodyNodes()));
joint->setName("joint_#" + std::to_string(skel->getNumJoints()));
joint->setTransformFromParentBodyNode(relTf);
for (size_t i = 0; i < joint->getNumDofs(); ++i)
joint->getDof(i)->setDampingCoefficient(DefaultDamping);
bn->createShapeNodeWith<
dart::dynamics::VisualAspect,
dart::dynamics::CollisionAspect,
dart::dynamics::DynamicsAspect>(jointShape);
dart::dynamics::ShapeNode* block = bn->createShapeNodeWith<
dart::dynamics::VisualAspect,
dart::dynamics::CollisionAspect,
dart::dynamics::DynamicsAspect>(mBlockShape);
block->setRelativeTransform(mBlockOffset);
dart::dynamics::Inertia inertia = bn->getInertia();
inertia.setMass(DefaultBlockMass);
inertia.setMoment(mBlockShape->computeInertia(DefaultBlockMass));
inertia.setLocalCOM(DefaultBlockLength / 2.0 * Eigen::Vector3d::UnitX());
bn->setInertia(inertia);
getWorld()->getConstraintSolver()->getCollisionGroup()->addShapeFramesOf(
bn);
clearPick();
return std::make_pair(joint, bn);
}
void customPreRefresh() override
{
if (isSimulating())
{
setAllBodyColors(DefaultSimulationColor);
setPickedNodeColor(DefaultForceBodyColor);
}
else
{
setAllBodyColors(DefaultPausedColor);
setPickedNodeColor(DefaultSelectedColor);
}
resetForceLine();
}
void p7142sd3c::startFilters() {
boost::recursive_mutex::scoped_lock guard(_p7142Mutex);
if (isSimulating())
return;
// Enable all adc fifos by setting the global gate enable
enableGateGen();
// Start the DDC coefficient counters
P7142_REG_WRITE(_BAR2Base + KAISER_ADDR, DDC_START);
usleep(p7142::P7142_IOCTLSLEEPUS);
DLOG << "fifos and filters enabled";
}
void p7142sd3c::stopFilters() {
boost::recursive_mutex::scoped_lock guard(_p7142Mutex);
if (isSimulating())
return;
uint32_t temp;
// stop the filters if they are running.
P7142_REG_READ (_BAR2Base + KAISER_ADDR, temp);
usleep(p7142::P7142_IOCTLSLEEPUS);
P7142_REG_WRITE(_BAR2Base + KAISER_ADDR, DDC_STOP);
usleep(p7142::P7142_IOCTLSLEEPUS);
P7142_REG_READ (_BAR2Base + KAISER_ADDR, temp);
usleep(p7142::P7142_IOCTLSLEEPUS);
// Disable all adc fifos by clearing the global gate enable
disableGateGen();
DLOG << "fifos and filters disabled, temp: " << temp;
}
void deletePick()
{
if (!mPickedNode)
return;
if (isSimulating())
{
std::cout << " -- Please pause simulation [using the Spacebar] before "
<< "attempting to delete blocks." << std::endl;
return;
}
dart::dynamics::SkeletonPtr temporary = mPickedNode->remove();
for (size_t i = 0; i < temporary->getNumBodyNodes(); ++i)
{
mViewer->disableDragAndDrop(
mViewer->enableDragAndDrop(temporary->getBodyNode(i)));
}
getWorld()->getConstraintSolver()->getCollisionGroup()->removeShapeFramesOf(
temporary.get());
clearPick();
}
