def set_target(self, pos, ori): joints = self.get_joint_angle() th, dist = robotlib.inv_kin(self._name, joints, pos, ori) err = np.linalg.norm(dist[0:3]) if err < 0.01: self._target_position = th return True else: print "[INVKIN] Failed at "+str(dist) return False
def move_to_pose(self, pos, ori): joints = self.get_joint_angle() th, dist = robotlib.inv_kin(self._name, joints, pos, [ori[0], ori[1], 0]) err = np.linalg.norm(dist[0:3]) if err < 0.01: print ori[2] th[6] += ori[2] self.set_joint_angle(th) return True print "[INVKIN] Failed at "+str(dist) return False