search

zitongbai / bipedal_control

Bipedal robot control using ocs2
BSD 3-Clause "New" or "Revised" License
37 stars 3 forks source link

pinocchio算正运动学时，末端位置很奇怪 #7

Open zitongbai opened 2 weeks ago

zitongbai commented 2 weeks ago

导入openloong机器人时，出现问题

pinocchio计算的正运动学结果中，脚的位置在base link的正下方，但是在rviz中（由robot_state_publisher维护的tf）显示的很正常

zitongbai commented 2 weeks ago

貌似pinocchio中把左右腿的关节弄反了

zitongbai commented 2 weeks ago

经过测试，pinocchio导入模型时，并没有完全按照urdf中的顺序。例如

robot name is: OpenLoong
---------- Successfully Parsed XML ---------------
root Link: base_link has 4 child(ren)
    child(1):  Link_arm_l_01
        child(1):  Link_arm_l_02
            child(1):  Link_arm_l_03
                child(1):  Link_arm_l_04
                    child(1):  Link_arm_l_05
                        child(1):  Link_arm_l_06
                            child(1):  Link_arm_l_07
    child(2):  Link_arm_r_01
        child(1):  Link_arm_r_02
            child(1):  Link_arm_r_03
                child(1):  Link_arm_r_04
                    child(1):  Link_arm_r_05
                        child(1):  Link_arm_r_06
                            child(1):  Link_arm_r_07
    child(3):  Link_head_yaw
        child(1):  Link_head_pitch
    child(4):  Link_waist_pitch
        child(1):  Link_waist_roll
            child(1):  Link_waist_yaw
                child(1):  Link_hip_l_roll
                    child(1):  Link_hip_l_yaw
                        child(1):  Link_hip_l_pitch
                            child(1):  Link_knee_l_pitch
                                child(1):  Link_ankle_l_pitch
                                    child(1):  Link_ankle_l_roll
                child(2):  Link_hip_r_roll
                    child(1):  Link_hip_r_yaw
                        child(1):  Link_hip_r_pitch
                            child(1):  Link_knee_r_pitch
                                child(1):  Link_ankle_r_pitch
                                    child(1):  Link_ankle_r_roll

在urdf中，是先右腿，再左腿

但是，在将urdf导入到pinocchio中时，模型中的关节顺序变成了先左腿，再右腿：

universe
root_joint
J_hip_l_roll
J_hip_l_yaw
J_hip_l_pitch
J_knee_l_pitch
J_ankle_l_pitch
J_ankle_l_roll
J_hip_r_roll
J_hip_r_yaw
J_hip_r_pitch
J_knee_r_pitch
J_ankle_r_pitch
J_ankle_r_roll

而代码中关节顺序很重要。关节顺序在各个机器人的task.info中model_settings.jointNames规定。

最开始是按照urdf中的顺序填写jointNames的，因此导致和pinocchio模型中的不一致

zitongbai commented 2 weeks ago

写一个脚本，输出pinocchio模型中关节的顺序