def solve_ik_for_arm(arm_index, arm_state, cur_arm_pose=None):
'''Finds an IK solution for a particular arm pose'''
# We need to find IK only if the frame is relative to an object
if (arm_state.refFrame == ArmState.OBJECT):
#rospy.loginfo('solve_ik_for_arm: Arm ' + str(arm_index) + ' is relative')
solution = ArmState()
target_pose = World.transform(arm_state.ee_pose,
arm_state.refFrameObject.name,
'base_link')
target_joints = Arms.arms[arm_index].get_ik_for_ee(
target_pose, arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for relative end-effector pose.')
return solution, False
else:
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = Pose(target_pose.position,
target_pose.orientation)
solution.joint_pose = target_joints
return solution, True
elif (arm_state.refFrame == ArmState.ROBOT_BASE):
#rospy.loginfo('solve_ik_for_arm: Arm ' + str(arm_index) + ' is absolute')
target_joints = Arms.arms[arm_index].get_ik_for_ee(
arm_state.ee_pose, arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for absolute end-effector pose.')
return arm_state, False
else:
solution = ArmState()
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation)
solution.joint_pose = target_joints
return solution, True
elif (arm_state.refFrame == ArmState.PREVIOUS_POSE):
#Calculate new arm state offset based on previous arm state
solution = ArmState()
target_ee_pose = Pose(
Point(cur_arm_pose.position.x + arm_state.ee_pose.position.x,
cur_arm_pose.position.y + arm_state.ee_pose.position.y,
cur_arm_pose.position.z + arm_state.ee_pose.position.z),
Quaternion(cur_arm_pose.orientation.x,
cur_arm_pose.orientation.y,
cur_arm_pose.orientation.z,
cur_arm_pose.orientation.w))
target_joints = Arms.arms[arm_index].get_ik_for_ee(
target_ee_pose, arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for relative end-effector pose.')
return solution, False
else:
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = target_ee_pose
solution.joint_pose = target_joints
return solution, True
else:
return arm_state, True
def solve_ik_for_arm(arm_index, arm_state, cur_arm_pose=None):
'''Finds an IK solution for a particular arm pose'''
# We need to find IK only if the frame is relative to an object
if (arm_state.refFrame == ArmState.OBJECT):
#rospy.loginfo('solve_ik_for_arm: Arm ' + str(arm_index) + ' is relative')
solution = ArmState()
target_pose = World.transform(arm_state.ee_pose,
arm_state.refFrameObject.name, 'base_link')
target_joints = Arms.arms[arm_index].get_ik_for_ee(target_pose,
arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for relative end-effector pose.')
return solution, False
else:
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = Pose(target_pose.position,
target_pose.orientation)
solution.joint_pose = target_joints
return solution, True
elif (arm_state.refFrame == ArmState.ROBOT_BASE):
#rospy.loginfo('solve_ik_for_arm: Arm ' + str(arm_index) + ' is absolute')
target_joints = Arms.arms[arm_index].get_ik_for_ee(
arm_state.ee_pose,
arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for absolute end-effector pose.')
return arm_state, False
else:
solution = ArmState()
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation)
solution.joint_pose = target_joints
return solution, True
elif (arm_state.refFrame == ArmState.PREVIOUS_POSE):
#Calculate new arm state offset based on previous arm state
solution = ArmState()
target_ee_pose = Pose(Point(
cur_arm_pose.position.x + arm_state.ee_pose.position.x,
cur_arm_pose.position.y + arm_state.ee_pose.position.y,
cur_arm_pose.position.z + arm_state.ee_pose.position.z),
Quaternion(cur_arm_pose.orientation.x,
cur_arm_pose.orientation.y,
cur_arm_pose.orientation.z,
cur_arm_pose.orientation.w))
target_joints = Arms.arms[arm_index].get_ik_for_ee(target_ee_pose,
arm_state.joint_pose)
if (target_joints == None):
rospy.logerr('No IK for relative end-effector pose.')
return solution, False
else:
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = target_ee_pose
solution.joint_pose = target_joints
return solution, True
else:
return arm_state, True
def _copy_arm_state(arm_state):
'''Makes a copy of the arm state'''
copy = ArmState()
copy.refFrame = int(arm_state.refFrame)
copy.joint_pose = arm_state.joint_pose[:]
copy.ee_pose = Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation)
## WARNING: the following is not really copying
copy.refFrameObject = arm_state.refFrameObject
return copy
def _copy_arm_state(arm_state):
'''Makes a copy of the arm state'''
copy = ArmState()
copy.refFrame = int(arm_state.refFrame)
copy.joint_pose = arm_state.joint_pose[:]
copy.ee_pose = Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation)
## WARNING: the following is not really copying
copy.refFrameObject = arm_state.refFrameObject
return copy
def _copy_arm_state(arm_state):
'''Returns a copy of arm_state.
Args:
arm_state (ArmState)
Returns:
ArmState
'''
copy = ArmState()
copy.refFrame = int(arm_state.refFrame)
copy.joint_pose = arm_state.joint_pose[:]
copy.ee_pose = Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation)
# WARNING: the following is not really copying
copy.refFrameLandmark = arm_state.refFrameLandmark
return copy
def solve_ik_for_arm(arm_index, arm_state, z_offset=0.0):
'''Finds an IK solution for a particular arm pose.
Args:
arm_index (int): Side.RIGHT or Side.LEFT
arm_state (ArmState): The arm's state,
z_offset (float, optional): Offset to add to z-values of
pose positions. Defaults to 0.0.
Returns:
(ArmState, bool): Tuple of
the ArmState, which will be an updated ArmState object
with IK solved if an IK solution was found. If IK was
not found, what is returned will depend on whether the
reference frame is relative (ArmState.OBJECT) or
absolute (ArmState.ROBOT_BASE). If the reference frame
is relative and no IK was found, an empty ArmState is
returned. If the reference frame is absolute and no IK
was found, then the original passed arm_state is
returned;
the bool, which is whether an IK solution was
successfully found.
'''
# Ideally we need to find IK only if the frame is relative to an
# object, but users can edit absolute poses in the GUI to make
# them unreachable, so we try IK for absolute poses too.
if arm_state.refFrame == ArmState.OBJECT:
# Arm is relative.
solution = ArmState()
target_pose_state = world.convert_ref_frame(arm_state,
ArmState.ROBOT_BASE)
target_pose = target_pose_state.ee_pose
target_pose.position.z = target_pose.position.z + z_offset
# Try solving IK.
target_joints = Arms.arms[arm_index].get_ik_for_ee(
target_pose, arm_state.joint_pose)
# Check whether solution found.
if target_joints is None:
# No solution: RETURN EMPTY ArmState.
rospy.logdebug('No IK for relative end-effector pose.')
return solution, False
else:
# Found a solution; update the solution arm_state and
# return.
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = Pose(target_pose.position,
target_pose.orientation)
solution.joint_pose = target_joints
return solution, True
elif arm_state.refFrame == ArmState.ROBOT_BASE:
# Arm is absolute.
pos = arm_state.ee_pose.position
target_position = Point(pos.x, pos.y, pos.z + z_offset)
target_pose = Pose(target_position, arm_state.ee_pose.orientation)
# Try solving IK.
target_joints = Arms.arms[arm_index].get_ik_for_ee(
target_pose, arm_state.joint_pose)
if target_joints is None:
# No IK found; return the original.
rospy.logdebug('No IK for absolute end-effector pose.')
return arm_state, False
else:
# IK found; fill in solution ArmState and return.
solution = ArmState()
solution.refFrame = ArmState.ROBOT_BASE
solution.ee_pose = Pose(arm_state.ee_pose.position,
arm_state.ee_pose.orientation)
solution.joint_pose = target_joints
return solution, True
else:
return arm_state, True
