예제 #1
0
Vector3<> ViewBikeMath::calcFootPos(const float& leg0, const float& leg1, const float& leg2, const float& leg3, const float& leg4, const float& leg5, const JointData::Joint& joint, const RobotDimensions& robotDimensions)
{
  Pose3D footPos(0, 0, 0);
  footPos.translate(0, 0, -robotDimensions.heightLeg5Joint);
  float sign = joint == JointData::LHipYawPitch ? 1.f : -1.f;
  footPos.translate(0, sign * (robotDimensions.lengthBetweenLegs / 2), 0);
  footPos.rotateX(-pi_4 * sign);
  footPos.rotateZ(-leg0 * sign);
  footPos.rotateX(pi_4 * sign);
  footPos.rotateX(-leg1 * sign);
  footPos.rotateY(leg2);
  footPos.translate(0, 0, -robotDimensions.upperLegLength);
  footPos.rotateY(leg3);
  footPos.translate(0, 0, -robotDimensions.lowerLegLength);
  return Vector3<>(footPos.translation.x, footPos.translation.y, footPos.translation.z);
}
예제 #2
0
Vector3f KickViewMath::calcFootPos(const float& leg0, const float& leg1, const float& leg2, const float& leg3, const float& leg4, const float&
                                   leg5, const Joints::Joint& joint, const RobotDimensions& robotDimensions)
{
  Pose3f footPos(0, 0, 0);
  footPos.translate(0, 0, -robotDimensions.footHeight);
  float sign = joint == Joints::lHipYawPitch ? 1.f : -1.f;
  footPos.translate(0, sign * (robotDimensions.yHipOffset), 0);
  footPos.rotateX(-pi_4 * sign);
  footPos.rotateZ(-leg0 * sign);
  footPos.rotateX(pi_4 * sign);
  footPos.rotateX(-leg1 * sign);
  footPos.rotateY(leg2);
  footPos.translate(0, 0, -robotDimensions.upperLegLength);
  footPos.rotateY(leg3);
  footPos.translate(0, 0, -robotDimensions.lowerLegLength);
  return Vector3f(footPos.translation.x(), footPos.translation.y(), footPos.translation.z());
}
예제 #3
0
int main()
{
    //Model Initialization
    Leph::HumanoidFixedModel model(Leph::SigmabanModel);

    //Inverse kinematics target
    bool isSuccess;
    Eigen::Vector3d trunkPos(0.1, -0.05, 0.20);
    Eigen::Vector3d trunkAxis(0.0, 0.0, -0.3);
    Eigen::Vector3d footPos(0.0, -0.10, 0.0);
    Eigen::Vector3d footAxis(0.0, 0.0, -0.6);

    Leph::ModelViewer viewer(1200, 900);
    double freq = 50.0;
    Leph::Scheduling scheduling;
    scheduling.setFrequency(freq);
    Leph::Plot plot;
    double t = 0.0;
    while (viewer.update()) {
        //Display model
        Leph::ModelDraw(model.get(), viewer);
        //Waiting
        scheduling.wait();

        //Update inverse kinematics
        if (t >= 3.0) {
            trunkPos.y() = -0.02 + 0.05*sin(1.5*3.14*0.1*t);
            footAxis.z() = -0.4 + 0.4*sin(2.0*3.14*0.1*t);
        }
        isSuccess = model.trunkFootIK(
            Leph::HumanoidFixedModel::LeftSupportFoot, 
            trunkPos,
            Leph::AxisToMatrix(trunkAxis),
            footPos,
            Leph::AxisToMatrix(footAxis));
        if (!isSuccess) {
            std::cout << "IK Error" << std::endl;
            return 1;
        }
        model.get().setDOF("base_x", 0.2*sin(3.0*3.14*t*0.1));
        model.get().setDOF("base_y", 0.2);
        model.get().setDOF("base_yaw", 1.5*sin(2.0*3.14*t*0.1));

        //Left support
        std::cout << "LEFT SUPPORT:" << std::endl;
        Eigen::VectorXd forceLeftSupport(6);
        Eigen::VectorXd forceLeftFoot(6);
        model.setSupportFoot(Leph::HumanoidFixedModel::LeftSupportFoot);
        Eigen::VectorXd tauDoubleLeft = 
            model.get().inverseDynamicsClosedLoop("right_foot_tip", &forceLeftFoot, false);
        //Convert support contact force 
        //in left support foot frame
        Eigen::Matrix3d matLeft = model.get().orientation("left_foot_tip", "origin");
        forceLeftSupport(3) = tauDoubleLeft(0);
        forceLeftSupport(4) = tauDoubleLeft(1);
        forceLeftSupport(5) = tauDoubleLeft(2);
        forceLeftSupport(0) = tauDoubleLeft(5);
        forceLeftSupport(1) = tauDoubleLeft(4);
        forceLeftSupport(2) = tauDoubleLeft(3);
        forceLeftSupport.segment(3, 3) = matLeft * forceLeftSupport.segment(3, 3);

        //Right support
        std::cout << "RIGHT SUPPORT:" << std::endl;
        Eigen::VectorXd forceRightSupport(6);
        Eigen::VectorXd forceRightFoot(6);
        model.setSupportFoot(Leph::HumanoidFixedModel::RightSupportFoot);
        Eigen::VectorXd tauDoubleRight = 
            model.get().inverseDynamicsClosedLoop("left_foot_tip", &forceRightFoot, false);
        //Convert support contact force 
        //in right support foot frame
        Eigen::Matrix3d matRight = model.get().orientation("right_foot_tip", "origin");
        forceRightSupport(3) = tauDoubleRight(0);
        forceRightSupport(4) = tauDoubleRight(1);
        forceRightSupport(5) = tauDoubleRight(2);
        forceRightSupport(0) = tauDoubleRight(5);
        forceRightSupport(1) = tauDoubleRight(4);
        forceRightSupport(2) = tauDoubleRight(3);
        forceRightSupport.segment(3, 3) = matRight * forceRightSupport.segment(3, 3);

        //Check that double support torques are the same
        //when computed from left and right support (kinematic root)
        for (const std::string& name : Leph::NamesDOF) {
            model.setSupportFoot(Leph::HumanoidFixedModel::LeftSupportFoot);
            size_t indexLeft = model.get().getDOFIndex(name);
            model.setSupportFoot(Leph::HumanoidFixedModel::RightSupportFoot);
            size_t indexRight = model.get().getDOFIndex(name);
            if (fabs(tauDoubleLeft(indexLeft)-tauDoubleRight(indexRight)) > 1e-6) {
                std::cout << "ERROR mismatch TORQUE name=" << name << std::endl;
                std::cout << name << " "
                    << "Left:" << indexLeft << ": " << tauDoubleLeft(indexLeft) 
                    << " Right:" << indexRight << ": " << tauDoubleRight(indexRight) << std::endl;
                exit(1);
            } 
        }

        //Check that contact forces computed from support foot
        //and from flying foot with the other 
        //support foot are the same
        if ((forceLeftSupport-forceRightFoot).norm() > 1e-5) {
            std::cout << "ERROR mismatch FORCE left foot: " << std::endl;
            std::cout << forceLeftSupport.transpose() << std::endl;
            std::cout << forceRightFoot.transpose() << std::endl;
            exit(1);
        }
        if ((forceRightSupport-forceLeftFoot).norm() > 1e-5) {
            std::cout << "ERROR mismatch FORCE right foot: " << std::endl;
            std::cout << forceRightSupport.transpose() << std::endl;
            std::cout << forceLeftFoot.transpose() << std::endl;
            exit(1);
        }

        std::cout << "LeftSupport:  " 
            << forceLeftSupport(0) << " " 
            << forceLeftSupport(1) << " " 
            << forceLeftSupport(2) << " " 
            << forceLeftSupport(3) << " " 
            << forceLeftSupport(4) << " " 
            << forceLeftSupport(5) << " " 
            << std::endl;
        std::cout << "RightFoot:    " 
            << forceRightFoot(0) << " " 
            << forceRightFoot(1) << " " 
            << forceRightFoot(2) << " " 
            << forceRightFoot(3) << " " 
            << forceRightFoot(4) << " " 
            << forceRightFoot(5) << " " 
            << std::endl;
        std::cout << "LeftFoot:     " 
            << forceLeftFoot(0) << " " 
            << forceLeftFoot(1) << " " 
            << forceLeftFoot(2) << " " 
            << forceLeftFoot(3) << " " 
            << forceLeftFoot(4) << " " 
            << forceLeftFoot(5) << " " 
            << std::endl;
        std::cout << "RightSupport: " 
            << forceRightSupport(0) << " " 
            << forceRightSupport(1) << " " 
            << forceRightSupport(2) << " " 
            << forceRightSupport(3) << " " 
            << forceRightSupport(4) << " " 
            << forceRightSupport(5) << " " 
            << std::endl;
        t += 0.01;
    }

    return 0;
}