def move_to(self, des_pos):
desired_pose = inverse_kinematics.get_pose_stamped(des_pos[0],
des_pos[1],
des_pos[2],
inverse_kinematics.EXAMPLE_O)
start_joints = self.ctrl.limb.joint_angles()
try:
des_joint_angles = inverse_kinematics.get_joint_angles(desired_pose, seed_cmd=start_joints,
use_advanced_options=True)
except ValueError:
rospy.logerr('no inverse kinematics solution found, '
'going to reset robot...')
current_joints = self.ctrl.limb.joint_angles()
self.ctrl.limb.set_joint_positions(current_joints)
raise Traj_aborted_except('raising Traj_aborted_except')
# self.move_with_impedance(des_joint_angles)
self.ctrl.set_joint_positions(des_joint_angles)
def move_to(self, des_pos, interp=True):
desired_pose = inverse_kinematics.get_pose_stamped(
des_pos[0], des_pos[1], des_pos[2], inverse_kinematics.EXAMPLE_O)
start_joints = self.ctrl.limb.joint_angles()
try:
des_joint_angles = inverse_kinematics.get_joint_angles(
desired_pose, seed_cmd=start_joints, use_advanced_options=True)
except ValueError:
rospy.logerr('no inverse kinematics solution found, '
'going to reset robot...')
current_joints = self.recorder.get_joint_angles()
des_joint_angles = current_joints
if interp:
self.ctrl.set_joint_positions_interp(des_joint_angles)
else:
self.ctrl.limb.set_joint_position_speed(0.15)
self.ctrl.set_joint_positions(des_joint_angles)
def get_interpolated_joint_angles(self):
int_des_pos = self.calc_interpolation(self.previous_des_pos, self.des_pos, self.t_prev, self.t_next)
# print 'interpolated: ', int_des_pos
desired_pose = inverse_kinematics.get_pose_stamped(int_des_pos[0],
int_des_pos[1],
int_des_pos[2],
inverse_kinematics.EXAMPLE_O)
start_joints = self.ctrl.limb.joint_angles()
try:
des_joint_angles = inverse_kinematics.get_joint_angles(desired_pose, seed_cmd=start_joints,
use_advanced_options=True)
except ValueError:
rospy.logerr('no inverse kinematics solution found, '
'going to reset robot...')
current_joints = self.ctrl.limb.joint_angles()
self.ctrl.limb.set_joint_positions(current_joints)
raise Traj_aborted_except('raising Traj_aborted_except')
return des_joint_angles
def pose_to_ja(target_pose,
start_joints,
tolerate_ik_error=False,
retry_on_fail=False,
debug_z=None):
try:
return inverse_kinematics.get_joint_angles(target_pose,
seed_cmd=start_joints,
use_advanced_options=True)
except ValueError:
if retry_on_fail:
logging.getLogger('robot_logger').error(
'retyring zangle was: {}'.format(debug_z))
return pose_to_ja(target_pose, NEUTRAL_JOINT_CMD)
elif tolerate_ik_error:
raise ValueError("IK failure") # signals to agent it should reset
else:
logging.getLogger('robot_logger').error(
'zangle was {}'.format(debug_z))
raise EnvironmentError(
"IK Failure") # agent doesn't handle EnvironmentError