void BodyLinkViewImpl::doInverseKinematics(Vector3 p, Matrix3 R)
{
InverseKinematicsPtr ik = currentBodyItem->getCurrentIK(currentLink);
if(ik){
currentBodyItem->beginKinematicStateEdit();
if(KinematicsBar::instance()->isPenetrationBlockMode()){
PenetrationBlockerPtr blocker = currentBodyItem->createPenetrationBlocker(currentLink, true);
if(blocker){
Position T;
T.translation() = p;
T.linear() = R;
if(blocker->adjust(T, Vector3(p - currentLink->p()))){
p = T.translation();
R = T.linear();
}
}
}
if(ik->calcInverseKinematics(p, R)){
currentBodyItem->notifyKinematicStateChange(true);
currentBodyItem->acceptKinematicStateEdit();
}
}
}
void BodyItemImpl::getDefaultIK(Link* targetLink, InverseKinematicsPtr& ik)
{
const Mapping& setupMap = *body->info()->findMapping("defaultIKsetup");
if(targetLink && setupMap.isValid()){
const Listing& setup = *setupMap.findListing(targetLink->name());
if(setup.isValid() && !setup.empty()){
Link* baseLink = body->link(setup[0].toString());
if(baseLink){
if(setup.size() == 1){
ik = getCustomJointPath(body, baseLink, targetLink);
} else {
CompositeIKPtr compositeIK(new CompositeIK(body, targetLink));
ik = compositeIK;
for(int i=0; i < setup.size(); ++i){
Link* baseLink = body->link(setup[i].toString());
if(baseLink){
if(!compositeIK->addBaseLink(baseLink)){
ik.reset();
break;
}
}
}
}
}
}
}
}
//==============================================================================
void InputHandler::initialize()
{
mRestConfig = mWam->getPositions();
for (std::size_t i=0; i < mWam->getNumEndEffectors(); ++i)
{
const InverseKinematicsPtr ik = mWam->getEndEffector(i)->getIK();
if (ik)
{
mDefaultBounds.push_back(ik->getErrorMethod().getBounds());
mDefaultTargetTf.push_back(ik->getTarget()->getRelativeTransform());
mConstraintActive.push_back(false);
mEndEffectorIndex.push_back(i);
}
}
}
void EditableSceneBodyImpl::makeLinkAttitudeLevel()
{
if(pointedSceneLink){
Link* targetLink = outlinedLink->link();
InverseKinematicsPtr ik = bodyItem->getCurrentIK(targetLink);
if(ik){
const Position& T = targetLink->T();
const double theta = acos(T(2, 2));
const Vector3 z(T(0,2), T(1, 2), T(2, 2));
const Vector3 axis = z.cross(Vector3::UnitZ()).normalized();
const Matrix3 R2 = AngleAxisd(theta, axis) * T.linear();
bodyItem->beginKinematicStateEdit();
if(ik->calcInverseKinematics(targetLink->p(), R2)){
bodyItem->notifyKinematicStateChange(true);
bodyItem->acceptKinematicStateEdit();
}
}
}
}
void initialize()
{
mRestConfig = mAtlas->getPositions();
mLegs.reserve(12);
for(size_t i=0; i<mAtlas->getNumDofs(); ++i)
{
if(mAtlas->getDof(i)->getName().substr(1, 5) == "_leg_")
mLegs.push_back(mAtlas->getDof(i)->getIndexInSkeleton());
}
// We should also adjust the pelvis when detangling the legs
mLegs.push_back(mAtlas->getDof("rootJoint_rot_x")->getIndexInSkeleton());
mLegs.push_back(mAtlas->getDof("rootJoint_rot_y")->getIndexInSkeleton());
mLegs.push_back(mAtlas->getDof("rootJoint_pos_z")->getIndexInSkeleton());
for(size_t i=0; i < mAtlas->getNumEndEffectors(); ++i)
{
const InverseKinematicsPtr ik = mAtlas->getEndEffector(i)->getIK();
if(ik)
{
mDefaultBounds.push_back(ik->getErrorMethod().getBounds());
mDefaultTargetTf.push_back(ik->getTarget()->getRelativeTransform());
mConstraintActive.push_back(false);
mEndEffectorIndex.push_back(i);
}
}
mPosture = std::dynamic_pointer_cast<RelaxedPosture>(
mAtlas->getIK(true)->getObjective());
mBalance = std::dynamic_pointer_cast<dart::constraint::BalanceConstraint>(
mAtlas->getIK(true)->getProblem()->getEqConstraint(1));
mOptimizationKey = 'r';
mMoveComponents.resize(TeleoperationWorld::NUM_MOVE, false);
}
