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
