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