def FK_all(self, arm, q, link_number = 7): q = self.meka_angles_to_kdl(arm, q) frame = self.FK_kdl(arm, q, link_number) pos = frame.p pos = ku.kdl_vec_to_np(pos) m = frame.M rot = ku.kdl_rot_to_np(m) return pos, rot
def FK_all(self, arm, q, link_number=7): q = self.meka_angles_to_kdl(arm, q) frame = self.FK_kdl(arm, q, link_number) pos = frame.p pos = ku.kdl_vec_to_np(pos) m = frame.M rot = ku.kdl_rot_to_np(m) return pos, rot
def FK_all(self, arm, q, link_number=7): q = self.pr2_to_kdl(q) frame = self.FK_kdl(arm, q, link_number) pos = frame.p pos = ku.kdl_vec_to_np(pos) pos = pos + self.right_arm_base_offset_from_torso_lift_link m = frame.M rot = ku.kdl_rot_to_np(m) return pos, rot
def FK_all(self, arm, q, link_number = 7): q = self.pr2_to_kdl(q) frame = self.FK_kdl(arm, q, link_number) pos = frame.p pos = ku.kdl_vec_to_np(pos) pos = pos + self.right_arm_base_offset_from_torso_lift_link m = frame.M rot = ku.kdl_rot_to_np(m) return pos, rot
def FK_vanilla(self, q, link_number=None): if link_number == None: link_number = self.n_jts link_number = min(link_number, self.n_jts) q = self.list_to_kdl(q) frame = self.FK_kdl(q, link_number) pos = frame.p pos = ku.kdl_vec_to_np(pos) pos = pos + self.linkage_offset_from_ground m = frame.M rot = ku.kdl_rot_to_np(m) return pos, rot
def FK_all(self, arm, q, link_number = 7): q = self.pr2_to_kdl(q) frame = self.FK_kdl(arm, q, link_number) pos = frame.p pos = ku.kdl_vec_to_np(pos) pos = pos + self.right_arm_base_offset_from_torso_lift_link m = frame.M rot = ku.kdl_rot_to_np(m) if arm == 0: tooltip_baseframe = rot * self.right_tooltip pos += tooltip_baseframe else: rospy.logerr('Arm %d is not supported.'%(arm)) return None return pos, rot