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