search

HiroIshida / snippets

fraction of codes which may be grepped later
6 stars 3 forks source link

pr2 calibrationでcameraの外部パラメータは推定されているのか? #72

Open HiroIshida opened 4 weeks ago

HiroIshida commented 4 weeks ago 
from skrobot.models.pr2 import PR2
from skrobot.models.urdf import RobotModelFromURDF

pr2 = PR2()
pr2.reset_manip_pose()
q_reset = pr2.angle_vector()

def print_frame(frame):
    while frame is not None:
        print(frame)
        frame = frame.parent_link

calibrated_model = RobotModelFromURDF(urdf_file="./robot_calibrated_2023_11_21_13_49-pi_rotated.xml")
calibrated_model.angle_vector(q_reset)
uncalibrated_model = RobotModelFromURDF(urdf_file="./robot_uncalibrated.xml")
uncalibrated_model.angle_vector(q_reset)

print("calibrated=>")
print_frame(calibrated_model.head_mount_kinect_rgb_optical_frame)
print("uncalibrated=>")
print_frame(uncalibrated_model.head_mount_kinect_rgb_optical_frame)

されているぽい. ir link to rgb link の間

calibrated=>
#<Link:head_mount_kinect_rgb_optical_frame 0x7fb09964cc10 0.117 -0.020 1.770 / -1.6 -0.0 -1.6>
#<Link:head_mount_kinect_rgb_link 0x7fb099647c40 0.117 -0.020 1.770 / 0.0 -0.0 0.0>
#<Link:head_mount_kinect_ir_link 0x7fb099635220 0.130 0.012 1.772 / 0.0 -0.0 0.0>
#<Link:head_mount_link 0x7fb099635160 0.046 0.000 1.660 / 0.0 -0.0 0.0>
#<Link:head_plate_frame 0x7fb0c4baedc0 0.065 0.000 1.496 / 0.0 -0.0 0.0>
#<Link:head_tilt_link 0x7fb0c4bb2df0 0.001 0.000 1.472 / 0.0 0.9 0.0>
#<Link:head_pan_link 0x7fb0c4baea60 -0.067 0.000 1.472 / 0.0 -0.0 0.0>
#<Link:torso_lift_link 0x7fb0c4ba6880 -0.050 0.000 1.091 / 0.0 -0.0 0.0>
#<Link:base_link 0x7fb0c4c5d4f0 0.000 0.000 0.051 / 0.0 -0.0 0.0>
#<Link:base_footprint 0x7fb0c4c48b50 0.000 0.000 0.000 / 0.0 -0.0 0.0>
uncalibrated=>
#<Link:head_mount_kinect_rgb_optical_frame 0x7fb09942ccd0 0.130 -0.017 1.772 / -1.6 -0.0 -1.6>
#<Link:head_mount_kinect_rgb_link 0x7fb099423430 0.130 -0.017 1.772 / 0.0 -0.0 0.0>
#<Link:head_mount_kinect_ir_link 0x7fb0996352b0 0.130 0.012 1.772 / 0.0 -0.0 0.0>
#<Link:head_mount_link 0x7fb099417460 0.046 0.000 1.660 / 0.0 -0.0 0.0>
#<Link:head_plate_frame 0x7fb0994e5ac0 0.065 0.000 1.496 / 0.0 -0.0 0.0>
#<Link:head_tilt_link 0x7fb0994ecf10 0.001 0.000 1.472 / 0.0 0.9 0.0>
#<Link:head_pan_link 0x7fb0994e9b80 -0.067 0.000 1.472 / 0.0 -0.0 0.0>
#<Link:torso_lift_link 0x7fb0994e16d0 -0.050 0.000 1.091 / 0.0 -0.0 0.0>
#<Link:base_link 0x7fb0995d6370 0.000 0.000 0.051 / 0.0 -0.0 0.0>
#<Link:base_footprint 0x7fb09957bdf0 0.000 0.000 0.000 / 0.0 -0.0 0.0>
HiroIshida commented 4 weeks ago

ここでsensors/chainを定義している. ロボット全体のチェインを考えるのではなく, カメラについてはtorso_lift_link以降のみを考えている? image

HiroIshida commented 4 weeks ago 
applications@pr1040:/etc/ros/noetic/urdf$ grep head_mount_kinect_rgb_joint robot_calibrated_2023_11_21_13_49-pi_rotated.xml -A4
  <joint name="head_mount_kinect_rgb_joint" type="fixed">
    <origin xyz="-0.00696920244115 -0.0325191168453 -0.0116889085003" rpy="0.0180369961393 -0.029141984173 0.0309389769244"/>
    <parent link="head_mount_kinect_ir_link"/>
    <child link="head_mount_kinect_rgb_link"/>
  </joint>
applications@pr1040:/etc/ros/noetic/urdf$ grep head_mount_kinect_rgb_joint robot_uncalibrated.xml -A4
  <joint name="head_mount_kinect_rgb_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 -0.03 0"/>
    <parent link="head_mount_kinect_ir_link"/>
    <child link="head_mount_kinect_rgb_link"/>
  </joint>