def move_to_startpos(self, pos):
desired_pose = inverse_kinematics.get_pose_stamped(pos[0],
pos[1],
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')
try:
if self.use_robot:
if self.use_imp_ctrl:
self.imp_ctrl_release_spring(30)
self.move_with_impedance_sec(des_joint_angles)
else:
self.ctrl.limb.move_to_joint_positions(des_joint_angles)
except OSError:
rospy.logerr('collision detected, stopping trajectory, going to reset robot...')
rospy.sleep(.5)
raise Traj_aborted_except('raising Traj_aborted_except')
if self.ctrl.limb.has_collided():
rospy.logerr('collision detected!!!')
rospy.sleep(.5)
raise Traj_aborted_except('raising Traj_aborted_except')
def state_to_pose(xyz, quat):
"""
:param xyz: desired pose xyz
:param quat: quaternion around z angle in [w, x, y, z] format
:return: stamped pose
"""
quat = Quaternion_msg(w=quat[0], x=quat[1], y=quat[2], z=quat[3])
desired_pose = inverse_kinematics.get_pose_stamped(xyz[0], xyz[1], xyz[2],
quat)
return desired_pose
def apply_act(self, action_vec, i_act, move=True):
self.des_pos[:2] += action_vec[:2]
self.des_pos = self.truncate_pos(self.des_pos) # make sure not outside defined region
self.imp_ctrl_release_spring(120.)
close_cmd = action_vec[2]
if close_cmd != 0:
self.topen = i_act + close_cmd
self.ctrl.gripper.close()
self.gripper_closed = True
up_cmd = action_vec[3]
delta_up = .1
if up_cmd != 0:
self.t_down = i_act + up_cmd
self.des_pos[2] = self.lower_height + delta_up
self.gripper_up = True
if self.gripper_closed:
if i_act == self.topen:
self.ctrl.gripper.open()
print 'opening gripper'
self.gripper_closed = False
if self.gripper_up:
if i_act == self.t_down:
self.des_pos[2] = self.lower_height
print 'going down'
self.imp_ctrl_release_spring(30.)
self.gripper_up = False
if move:
desired_pose = inverse_kinematics.get_pose_stamped(self.des_pos[0],
self.des_pos[1],
self.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)
return self.des_pos
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 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 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