pinocchi刚体动力学库使用教程

最新推荐文章于 2024-06-21 20:56:56 发布

阿狸OKay

最新推荐文章于 2024-06-21 20:56:56 发布

阅读量237

点赞数 1

文章标签： linux 算法运维

本文链接：https://blog.csdn.net/weixin_50084948/article/details/138164691

版权

1. cmake文件写法

2. c++代码，求出雅可比矩阵，并求出人形机器人关节末端速度

1.touch CMakeLists.txt

cmake_minimum_required(VERSION 3.16)
project(test_pinocchi)

find_package(pinocchio)
find_package(eigen)

get_cmake_property(_variableNames VARIABLES)
foreach (_variableName ${_variableNames})
    message(STATUS "@@@ ${_variableName}=${${_variableName}}")
endforeach()

include_directories(/opt/openrobots/include)
include_directories(${EIGEN3_INCLUDE_DIR})
link_directories(/opt/openrobots/lib)


add_executable(loadmodel loadingModel.cpp)
add_executable(geometry geometry.cpp)
add_executable(ik ik.cpp)
add_executable(pin pin.cpp)
add_executable(reduced_model build_reduced_model.cpp)
add_executable(file_op file_op.cpp)

target_link_libraries(loadmodel pinocchio)
target_link_libraries(geometry pinocchio)
target_link_libraries(ik pinocchio)
target_link_libraries(pin pinocchio)
target_link_libraries(reduced_model pinocchio)

2. c++代码，求出雅可比矩阵，并求出人形机器人关节末端速度

void computePinForwadKinematic1(double q[12], double dq[12])
{

    Eigen::VectorXd q_full, dq_full;    
    q_full.setZero(12);
    dq_full.setZero(12);
    // q_full << 0,0,-0.175377, 0.58288,-0.407945,0,0,0,-0.175377, 0.58288,-0.407945,0;
    For(12)
    {
        q_full[i] = q[i];
        dq_full[i] = dq[i];
    }

    pinocchio::forwardKinematics(pinModel, pinData, q_full, dq_full);

    // std::cout << q_full<< " q_full "<<std::endl;

    pinocchio::updateFramePlacements(pinModel, pinData);

    pinocchio::FrameIndex l_foot_id_(pinModel.getFrameId(
        "Z6",
        (pinocchio::FrameType)(pinocchio::JOINT | pinocchio::FIXED_JOINT |
                               pinocchio::BODY)));

    pinocchio::FrameIndex r_foot_id_(pinModel.getFrameId(
        "Y6",
        (pinocchio::FrameType)(pinocchio::JOINT | pinocchio::FIXED_JOINT |
                               pinocchio::BODY)));

    // std::cout << r_foot_id_ << " r foot "<<std::endl; //16
    // std::cout << l_foot_id_ << " l foot "<<std::endl; //28

    // std::cout<<"before IK, ths placement is "<<
    //         pinData.oMf[r_foot_id_].translation().transpose()<<std::endl;

    const pinocchio::SE3::Vector3 &r_foot_pos0 =
        pinData.oMf[r_foot_id_].translation();
    const pinocchio::SE3::Vector3 &l_foot_pos0 =
        pinData.oMf[l_foot_id_].translation();
    // std::cout << r_foot_pos0 << " r foot p"<<std::endl;
    // std::cout << l_foot_pos0 << " l footp "<<std::endl;
    rightFootPlace << r_foot_pos0[0], r_foot_pos0[1], r_foot_pos0[2];
    leftFootPlace << l_foot_pos0[0], l_foot_pos0[1], l_foot_pos0[2];

    // 速度
    //  = pinData.oMf[l_foot_id_].translation();
    //zhhw
    // rightFootVelocity = pinData.ov[1 + 5].linear();  
    // leftFootVelocity = pinData.ov[1+6 + 5].linear(); 
    // outFile<<"*******rightFootVelocity"<<rightFootVelocity<<std::endl;//zhhw
    //zhhw
    // rightFootVelocity = pinData.v[1 + 5].linear();  
    // leftFootVelocity = pinData.v[1+6 + 5].linear();  
    pinocchio::Data::Matrix6x Jrframe(6,12);
    Jrframe.setZero();
    pinocchio::Data::Matrix6x Jlframe(6,12);
    Jlframe.setZero();
    pinocchio::computeFrameJacobian(pinModel, pinData, q_full, r_foot_id_, Jrframe);
    pinocchio::computeFrameJacobian(pinModel, pinData, q_full, l_foot_id_, Jlframe);

    // // std::cout << "pinModel.njoints: "<< pinModel.njoints <<std::endl;
        
    // rightFootVelocity = Jr * dq_full;  
    // leftFootVelocity =  Jl * dq_full;  
    // Eigen::VectorXd vel_tmp;
     pinocchio::Data::Matrix6x Jrframe_real(6,6);
    Jrframe_real.setZero();
    pinocchio::Data::Matrix6x Jlframe_real(6,6);
    Jlframe_real.setZero();

    Jrframe_real = Jrframe.block<6,6>(0,0);
    Jlframe_real = Jlframe.block<6,6>(0,6);

    // Eigen::MatrixXd r_vel_tmp,l_vel_tmp;
    r_vel_tmp.resize(6,1);
    l_vel_tmp.resize(6,1);
    // vel_tmp.setZero();
    r_vel_tmp << Jrframe_real * dq_full.block<6,1>(0,0);
    l_vel_tmp << Jlframe_real * dq_full.block<6,1>(6,0);    
    // leftFootVelocity =  Jlframe * dq_full;  
    // std::cout << "pinModel.njoints: "<< pinModel.njoints <<std::endl;
    // std::cout << "pinModel.nv: "<< pinModel.nv <<std::endl;
    std::cout << "rightFootVelocity: " << rightFootVelocity.transpose() << std::endl;
    std::cout << "leftFootVelocity:  " << leftFootVelocity.transpose() << std::endl;
    // std::cout << "pinModel.nv: "<< pinModel.nv <<std::endl;
    // std::cout << "rightFootVelocity: " << rightFootVelocity.transpose() << std::endl;
    // std::cout << "leftFootVelocity:  " << leftFootVelocity.transpose() << std::endl;
}