具身智能学习笔记

使用 Pinocchio 构建运动学模型

5 min read

  1. 参考 Simulation and Control
  2. 简洁方式构建运动学模型
from simulation_and_control import PinWrapper
 
dyn_model = PinWrapper(conf_file_name, "pybullet", ext_names, source_names, False,0,cur_dir)
num_joints = dyn_model.getNumberofActuatedJoints()
  1. PinWrapper：加载 URDF 模型 self._LoadPinURDF(urdf_file) 并设定自由度（DOF）
    • 固定底座：
      • self.n_b = 0：没有浮动基座的自由度。
      • self.n_bdot = 0：没有基座速度自由度。
      • self.n_q = self.pin_model.nq：配置关节自由度。
      • self.n_qdot = self.pin_model.nv：配置速度自由度。
      • self.n = self.n_q，self.n_dot = self.n_qdot：总自由度就是机械臂自身的自由度。
    • 自由底座：
      • self.n_b = 7：基座位置 (x, y, z) + 姿态 (四元数 4) = 7。
      • self.n_bdot = 6：基座速度自由度，平移速度 (3) + 角速度 (3)。
      • self.n_q = self.pin_model.nq - 7：去掉基座自由度，剩下的就是关节自由度。
      • self.n_qdot = self.pin_model.nv - 6：速度同理。
      • self.n = self.n_q + self.n_b：总的配置自由度。
      • self.n_dot = self.n_qdot + self.n_bdot：总的速度自由度。
# check if the robot is on rack and update the pinocchio model according
if self.base_type=="fixed":
    self.pin_model = pin.buildModelFromUrdf(urdf_file)
    self.pin_data = self.pin_model.createData()
    # here i assume that the robot has an actuate and an underactuated part
    # get the number of actuated joints
    self.n_b = 0     # floating base dof (x y z  [0 0 0 1])
    self.n_bdot = 0  # floating base velocity dof (x_dot y_dot z_dot  roll_dot yaw_dot pitch_dot)
    self.n_q = self.pin_model.nq      # joint dof
    self.n_qdot = self.pin_model.nv   # joint velocity dof
    self.n = self.n_q
    self.n_dot = self.n_qdot
else: # TODO add the case for on_rack (only CoM position is fixed orientation is free)
    # Load the urdf model for pinocchio with floating base
    self.pin_model = pin.buildModelFromUrdf(urdf_file,pin.JointModelFreeFlyer())
    self.pin_data = self.pin_model.createData()
    self.n_b = 7     # floating base dof (x y z  [0 0 0 1])
    self.n_bdot = 6  # floating base velocity dof (x_dot y_dot z_dot  omega_x omega_y omega_z) base frame
    self.n_q = self.pin_model.nq - 7      # joint dof
    self.n_qdot = self.pin_model.nv - 6   # joint velocity dof
    self.n = self.n_q + self.n_b
    self.n_dot = self.n_qdot + self.n_bdot
  1. PinWrapper：如果需要 visualizer，则创建
    • 应该是在没有仿真器的时候使用的
if self.visualizer:
    from pinocchio.visualize import GepettoVisualizer
    package_dir= os.path.dirname(urdf_file)  
    print("package_dir=",package_dir)    
    self.collision_model = pin.buildGeomFromUrdf(self.pin_model,urdf_file,pin.GeometryType.COLLISION,package_dir)
    #self.collision_model = pin.buildGeomFromUrdf(self.pin_model, urdf_file, pin.GeometryType.COLLISION)
    self.visual_model = pin.buildGeomFromUrdf(self.pin_model,urdf_file,pin.GeometryType.VISUAL,package_dir)
    #self.visual_model = pin.buildGeomFromUrdf(self.pin_model, urdf_file, pin.GeometryType.VISUAL)
    self.viz = GepettoVisualizer(self.pin_model, self.collision_model , self.visual_model)
    self.viz.initViewer()
    self.viz.loadViewerModel("pinocchio")
  1. PinWrapper：根据配置文件里的脚部 frame 名称，建立“标准脚名（FL/FR/RL/RR）→ 对应 frame id”的映射表。
self.feet_id[n] = self.pin_model.getFrameId(n)
  1. PinWrapper：对应机器人关节和 Pinocchio 模型关节
    • 检查是否需要 从机器人关节顺序 → pinocchio 模型关节顺序 的映射。
    • 如果需要，强制用户提供外部关节顺序（list_link_name_for_reodering）和数据源信息（data_source_names）。
    • 调用 CreateIndexJointAssociation 来建立双向映射 ext2pin 和 pin2ext。
    • 如果不启用转换，就直接不做处理（设为 None）。
self.CreateIndexJointAssociation(list_link_name_for_reodering.copy(),data_source_names.copy())
  1. PinWrapper： 根据配置文件里的 control_groups，把不同控制接口对应的关节名字映射到 Pinocchio 的 joint ID，方便后续分组控制。
    • 一个机器人可能有很多个关节，但并不是所有关节都由同一个控制器管理。所以需要 control_groups 这个机制，把不同关节分组，方便控制。比如：
      • 机械臂：有一个 group 管理 7 个关节。
      • 移动底盘：另一个 group 管理 4 个轮子的关节。
      • 夹爪：再一个 group 管理手爪的 2 个关节。
for name in control_group_names:
    joint_names_in_group = self.control_groups[name]
    cur_joint_id = []
    for joint_name in joint_names_in_group:
        cur_joint_id.append(self.pin_model.getJointId(joint_name))
    self.control_group_id[name] = cur_joint_id
  1. 计算初始关节角度
def ComputeFK(self,q,link_name):
    id = self.pin_model.getFrameId(link_name)
    # reorder from external to pinocchio
    q_ = self.ReoderJoints2PinVec(q,"pos")
    pin.forwardKinematics(self.pin_model, self.pin_data, q_)
    pin.updateFramePlacements(self.pin_model, self.pin_data)
    return self.pin_data.oMf[id].translation, self.pin_data.oMf[id].rotation
 
controlled_frame_name = "panda_link8"
init_joint_angles = sim.GetInitMotorAngles()
init_cartesian_pos,init_R = dyn_model.ComputeFK(init_joint_angles,controlled_frame_name)
# print init joint
print(f"Initial joint angles: {init_joint_angles}")

Graph View

Backlinks