Example #1
0
 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
Example #2
0
 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
Example #3
0
 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
Example #4
0
 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
Example #5
0
    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_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
Example #7
0
 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