bool ZMPFallDetector::getStabilityInidcator(SupportPhase phase, const Eigen::Matrix4f& leftFootPose, const Eigen::Matrix4f& rightFootPose) { // If not foot touches the ground, we are f****d. bool stillFalling = phase == SUPPORT_NONE; if (stillFalling) return true; const auto& supportHull = [this, phase, &leftFootPose, &rightFootPose]() { if (phase == Bipedal::SUPPORT_LEFT) return leftSupportHull; if (phase == Bipedal::SUPPORT_RIGHT) return rightSupportHull; BOOST_ASSERT(phase == Bipedal::SUPPORT_BOTH); // Foot positions changed //if (lastSupportPhase != phase) { dualSupportHull = computeSupportPolygone(leftFootPose, rightFootPose, leftSupportHull, rightSupportHull, phase); } lastSupportPhase = phase; return dualSupportHull; }(); // center of convex hull and orientation should correspond with ground frame const auto& groundFrame = Bipedal::computeGroundFrame(leftFootPose, rightFootPose, phase); Eigen::Vector3f zmp = Eigen::Vector3f::Zero(); zmp.head(2) = zmpEstimator.estimation * 1000; Eigen::Vector3f zmpConvexHull = VirtualRobot::MathTools::transformPosition(zmp, groundFrame.inverse()); // if zmp is inside of the CH, we are not falling if (VirtualRobot::MathTools::isInside(zmpConvexHull.head(2), supportHull)) { stillFalling = false; //std::cout << "(" << phase << ") ZMP is inside the CH. " << std::endl; } else { Eigen::Vector3f contact = Eigen::Vector3f::Zero(); contact.head(2) = Bipedal::computeHullContactPoint(zmpConvexHull.head(2), supportHull); contactPoint = VirtualRobot::MathTools::transformPosition(contact, groundFrame); double dist = (contact-zmpConvexHull).norm(); //std::cout << "(" << phase << ") ZMP is outside the CH: " << dist << std::endl; stillFalling = dist > maxHullDist; } return stillFalling; }
/** * Warning: This only works if we have a position without a slope! */ double ZMPDistributor::computeAlpha(const Eigen::Matrix4f& groundPoseLeft, const Eigen::Matrix4f& groundPoseRight, const Eigen::Vector3f& refZMP, const Eigen::Vector2f& relZMPLeft, const Eigen::Vector2f& relZMPRight, Bipedal::SupportPhase phase) { double alpha; Eigen::Vector2f leftContact, rightContact; Eigen::Vector2f pAlpha; switch (phase) { case Bipedal::SUPPORT_LEFT: alpha = 0.0; break; case Bipedal::SUPPORT_RIGHT: alpha = 1.0; break; case Bipedal::SUPPORT_BOTH: // get contact points in world coordinates leftContact = VirtualRobot::MathTools::transformPosition(Bipedal::computeHullContactPoint(relZMPLeft, leftConvexHull), groundPoseLeft); rightContact = VirtualRobot::MathTools::transformPosition(Bipedal::computeHullContactPoint(relZMPRight, rightConvexHull), groundPoseRight); pAlpha = VirtualRobot::MathTools::nearestPointOnSegment( leftContact, rightContact, Eigen::Vector2f(refZMP.head(2)) ); alpha = (leftContact - pAlpha).norm() / (leftContact - rightContact).norm(); break; } return alpha; }
IGL_INLINE void igl::viewer::ViewerCore::get_scale_and_shift_to_fit_mesh( const Eigen::MatrixXd& V, float& zoom, Eigen::Vector3f& shift) { if (V.rows() == 0) return; auto min_point = V.colwise().minCoeff(); auto max_point = V.colwise().maxCoeff(); auto centroid = (0.5*(min_point + max_point)).eval(); shift.setConstant(0); shift.head(centroid.size()) = -centroid.cast<float>(); zoom = 2.0 / (max_point-min_point).array().abs().maxCoeff(); }
/** * @brief Computes and applies the translation transformations to the trackball given new position. * @param pos New mouse position in normalized trackball system */ void translateCamera (const Eigen::Vector2f& pos) { Eigen::Vector2f normalized_pos = normalizePosition(pos); if (!translating) { translating = true; initialTranslationPosition = normalized_pos; } else if (pos != initialPosition.head(2)) { finalTranslationPosition = normalized_pos; computeTranslationVector(); updateViewMatrix(); initialTranslationPosition = finalTranslationPosition; } }
/** * @brief Computes and applies the rotation transformations to the trackball given new position. * @param pos New mouse position in normalized trackball system */ void rotateCamera (const Eigen::Vector2f& pos) { Eigen::Vector3f normalized_pos = computeSpherePosition(normalizePosition(pos)); if (!rotating) { rotating = true; initialPosition = normalized_pos; } else if ( pos != initialPosition.head(2)) { finalPosition = normalized_pos; computeRotationAngle(); updateViewMatrix(); initialPosition = finalPosition; } }
ZMPDistributor::ForceTorque ZMPDistributor::distributeZMP(const Eigen::Vector3f& localAnkleLeft, const Eigen::Vector3f& localAnkleRight, const Eigen::Matrix4f& leftFootPoseGroundFrame, const Eigen::Matrix4f& rightFootPoseGroundFrame, const Eigen::Vector3f& zmpRefGroundFrame, Bipedal::SupportPhase phase) { Eigen::Matrix4f groundPoseLeft = Bipedal::projectPoseToGround(leftFootPoseGroundFrame); Eigen::Matrix4f groundPoseRight = Bipedal::projectPoseToGround(rightFootPoseGroundFrame); Eigen::Vector3f localZMPLeft = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseLeft.inverse()); Eigen::Vector3f localZMPRight = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseRight.inverse()); double alpha = computeAlpha(groundPoseLeft, groundPoseRight, zmpRefGroundFrame, localZMPLeft.head(2), localZMPRight.head(2), phase); //std::cout << "########## " << alpha << " ###########" << std::endl; ForceTorque ft; // kg*m/s^2 = N ft.leftForce = -(1-alpha) * mass * gravity; ft.rightForce = -alpha * mass * gravity; // Note we need force as kg*mm/s^2 ft.leftTorque = (localAnkleLeft - localZMPLeft).cross(ft.leftForce * 1000); ft.rightTorque = (localAnkleRight - localZMPRight).cross(ft.rightForce * 1000); // ZMP not contained in convex polygone if (std::fabs(alpha) > std::numeric_limits<float>::epsilon() && std::fabs(alpha-1) > std::numeric_limits<float>::epsilon()) { Eigen::Vector3f leftTorqueGroundFrame = groundPoseLeft.block(0, 0, 3, 3) * ft.leftTorque; Eigen::Vector3f rightTorqueGroundFrame = groundPoseRight.block(0, 0, 3, 3) * ft.rightTorque; Eigen::Vector3f tau0 = -1 * (leftTorqueGroundFrame + rightTorqueGroundFrame); //std::cout << "Tau0World: " << tau0.transpose() << std::endl; //std::cout << "leftTorqueWorld: " << leftTorqueWorld.transpose() << std::endl; //std::cout << "rightTorqueWorld: " << rightTorqueWorld.transpose() << std::endl; // Note: Our coordinate system is different than in the paper! // Also this is not the same as the ground frame. Eigen::Vector3f xAxis = leftFootPoseGroundFrame.block(0,3,3,1) + localAnkleLeft - localAnkleRight - rightFootPoseGroundFrame.block(0,3,3,1); xAxis /= xAxis.norm(); Eigen::Vector3f zAxis(0, 0, 1); Eigen::Vector3f yAxis = zAxis.cross(xAxis); yAxis /= yAxis.norm(); Eigen::Matrix3f centerFrame; centerFrame.block(0, 0, 3, 1) = xAxis; centerFrame.block(0, 1, 3, 1) = yAxis; centerFrame.block(0, 2, 3, 1) = zAxis; //std::cout << "Center frame:\n" << centerFrame << std::endl; Eigen::Vector3f centerTau0 = centerFrame.transpose() * tau0; Eigen::Vector3f leftTorqueCenter; leftTorqueCenter.x() = (1-alpha)*centerTau0.x(); leftTorqueCenter.y() = centerTau0.y() < 0 ? centerTau0.y() : 0; leftTorqueCenter.z() = 0; Eigen::Vector3f rightTorqueCenter; rightTorqueCenter.x() = alpha*centerTau0.x(); rightTorqueCenter.y() = centerTau0.y() < 0 ? 0 : centerTau0.y(); rightTorqueCenter.z() = 0; //std::cout << "Tau0Center: " << centerTau0.transpose() << std::endl; //std::cout << "leftTorqueCenter: " << leftTorqueCenter.transpose() << std::endl; //std::cout << "rightTorqueCenter: " << rightTorqueCenter.transpose() << std::endl; // tcp <--- ground frame <--- center frame ft.leftTorque = groundPoseLeft.block(0, 0, 3, 3).transpose() * centerFrame * leftTorqueCenter; ft.rightTorque = groundPoseRight.block(0, 0, 3, 3).transpose() * centerFrame * rightTorqueCenter; } // Torque depends on timestep, we need a way to do this correctly. const double torqueFactor = 1; // convert to Nm ft.leftTorque *= torqueFactor / 1000.0 / 1000.0; ft.rightTorque *= torqueFactor / 1000.0 / 1000.0; //std::cout << "leftTorque: " << ft.leftTorque.transpose() << std::endl; //std::cout << "rightTorque: " << ft.rightTorque.transpose() << std::endl; return ft; }