def set_left_arm_pose(self, position, orientation):
'''
Move the left arm to a position
@param position: x,y,z
@param angles: quaternion
'''
req = GetPositionIKRequest()
req.timeout = rospy.Duration(5.0)
req.ik_request.ik_link_name = "l_wrist_roll_link"
req.ik_request.pose_stamped.header.frame_id = "base_link"
req.ik_request.pose_stamped.pose.position.x = position[0]
req.ik_request.pose_stamped.pose.position.y = position[1]
req.ik_request.pose_stamped.pose.position.z = position[2]
req.ik_request.pose_stamped.pose.orientation.x = orientation[0]
req.ik_request.pose_stamped.pose.orientation.y = orientation[1]
req.ik_request.pose_stamped.pose.orientation.z = orientation[2]
req.ik_request.pose_stamped.pose.orientation.w = orientation[3]
req.ik_request.ik_seed_state.joint_state.position = self.robot_state.left_arm_positions
req.ik_request.ik_seed_state.joint_state.name = self.robot_state.left_joint_names
res = self.left_ik(req)
if res.error_code.val != 1:
rospy.logerr("IK error, code is %d" % res.error_code.val)
return None
joints = res.solution.joint_state.position
return joints
def __init__(self):
ik_serv_name = '/compute_ik'
fk_serv_name = '/compute_fk'
self.frame_id = "torso_lift_link"
self.r_link_name = 'r_wrist_roll_link'
self.l_link_name = 'l_wrist_roll_link'
self.r_group_name = 'right_arm'
self.l_group_name = 'left_arm'
#Set up right/left FK service connections
print 'Waiting for forward kinematics services...'
rospy.wait_for_service(fk_serv_name)
self.getPosFK = rospy.ServiceProxy(fk_serv_name, GetPositionFK)
self.getPosFKPersistent = rospy.ServiceProxy(fk_serv_name,
GetPositionFK,
persistent=True)
print "OK"
#Set up right/left FK service requests
self.FKreq = [GetPositionFKRequest(), GetPositionFKRequest()]
self.FKreq[0].header.frame_id = self.frame_id
self.FKreq[0].fk_link_names = [self.r_link_name]
self.FKreq[0].robot_state.joint_state.name = self.getJointNames(0)
self.FKreq[1].header.frame_id = self.frame_id
self.FKreq[1].fk_link_names = [self.l_link_name]
self.FKreq[1].robot_state.joint_state.name = self.getJointNames(1)
#Set up right/left IK service connections
print 'Waiting for inverse kinematics services...'
rospy.wait_for_service(ik_serv_name)
self.getPosIK = rospy.ServiceProxy(ik_serv_name, GetPositionIK)
self.getPosIKPersistent = rospy.ServiceProxy(ik_serv_name,
GetPositionIK,
persistent=True)
print 'OK'
#Set up right/left IK service requests
self.IKreq = [GetPositionIKRequest(), GetPositionIKRequest()]
self.IKreq[0].ik_request.robot_state.joint_state.position = [0] * 7
self.IKreq[
0].ik_request.robot_state.joint_state.name = self.getJointNames(0)
self.IKreq[0].ik_request.group_name = self.r_group_name
self.IKreq[0].ik_request.ik_link_name = self.r_link_name
self.IKreq[0].ik_request.pose_stamped.header.frame_id = self.frame_id
self.IKreq[1].ik_request.robot_state.joint_state.position = [0] * 7
self.IKreq[
1].ik_request.robot_state.joint_state.name = self.getJointNames(1)
self.IKreq[1].ik_request.group_name = self.l_group_name
self.IKreq[1].ik_request.ik_link_name = self.l_link_name
self.IKreq[1].ik_request.pose_stamped.header.frame_id = self.frame_id
def compute_ik(self, pose_stamped, timeout=rospy.Duration(5)):
"""Computes inverse kinematics for the given pose.
Note: if you are interested in returning the IK solutions, we have
shown how to access them.
Args:
pose_stamped: geometry_msgs/PoseStamped.
timeout: rospy.Duration. How long to wait before giving up on the
IK solution.
Returns: A list of (name, value) for the arm joints if the IK solution
was found, False otherwise.
"""
request = GetPositionIKRequest()
request.ik_request.pose_stamped = pose_stamped
request.ik_request.group_name = 'arm'
request.ik_request.timeout = timeout
response = self._compute_ik(request)
error_str = moveit_error_string(response.error_code.val)
success = error_str == 'SUCCESS'
if not success:
return False
joint_state = response.solution.joint_state
joints = []
for name, position in zip(joint_state.name, joint_state.position):
if name in ArmJoints.names():
joints.append((name, position))
return joints
def getIkPose(self, pose, group="right_arm", previous_state=None):
"""Get IK of the pose specified, for the group specified, optionally using
the robot_state of previous_state (if not, current robot state will be requested) """
# point point to test if there is ik
# returns the answer of the service
rqst = GetPositionIKRequest()
rqst.ik_request.avoid_collisions = True
rqst.ik_request.group_name = group
rqst.ik_request.pose_stamped.header = Header(stamp=rospy.Time.now())
rqst.ik_request.pose_stamped.header.frame_id = 'base_link'
# Set point to check IK for
rqst.ik_request.pose_stamped.pose.position = pose.position
rqst.ik_request.pose_stamped.pose.orientation = pose.orientation
if previous_state == None:
current_joint_state = rospy.wait_for_message(
DEFAULT_JOINT_STATES, JointState)
cs = RobotState()
cs.joint_state = current_joint_state
#cs = self.r_commander.get_current_state() # old code
rqst.ik_request.robot_state = cs
else:
rqst.ik_request.robot_state = previous_state
ik_answer = GetPositionIKResponse()
if DEBUG_MODE:
timeStart = rospy.Time.now()
ik_answer = self.ik_serv.call(rqst)
if DEBUG_MODE:
durationCall = rospy.Time.now() - timeStart
rospy.loginfo("Call took: " + str(durationCall.to_sec()) + "s")
return ik_answer
def getIK(self, group_name, ik_link_name, pose, avoid_collisions=True, attempts=None, robot_state=None,
constraints=None):
"""Get the inverse kinematics for a group with a link a in pose in 3d world.
@group_name string group i.e. right_arm that will perform the IK
@ik_link_name string link that will be in the pose given to evaluate the IK
@pose PoseStamped that represents the pose (with frame_id!) of the link
@avoid_collisions Bool if we want solutions with collision avoidance
@attempts Int number of attempts to get an Ik as it can fail depending on what IK is being used
@robot_state RobotState the robot state where to start searching IK from (optional, current pose will be used
if ignored)"""
gpikr = GetPositionIKRequest()
gpikr.ik_request.group_name = group_name
if robot_state != None: # current robot state will be used internally otherwise
gpikr.ik_request.robot_state = robot_state
gpikr.ik_request.avoid_collisions = avoid_collisions
gpikr.ik_request.ik_link_name = ik_link_name
if type(pose) == type(PoseStamped()):
gpikr.ik_request.pose_stamped = pose
else:
rospy.logerr("pose is not a PoseStamped, it's: " + str(type(pose)) + ", can't ask for an IK")
return
if attempts != None:
gpikr.ik_request.attempts = attempts
else:
gpikr.ik_request.attempts = 0
if constraints != None:
gpikr.ik_request.constraints = constraints
ik_result = self.ik_srv.call(gpikr)
rospy.logwarn("Sent: " + str(gpikr))
return ik_result
def computeIKPossible(self, x, y, z, qx=0.707, qy=0.707, qz=0.0, qw=0.0):
req = GetPositionIKRequest()
req.ik_request.group_name = "manipulator"
req.ik_request.robot_state = RobotState()
# req.ik_request.robot_state.joint_state = self.joint_state
req.ik_request.avoid_collisions = True
req.ik_request.pose_stamped = PoseStamped()
req.ik_request.pose_stamped.header.frame_id = "base_link"
req.ik_request.pose_stamped.header.stamp = rospy.get_rostime()
req.ik_request.pose_stamped.pose.position.x = x
req.ik_request.pose_stamped.pose.position.y = y
req.ik_request.pose_stamped.pose.position.z = z
req.ik_request.pose_stamped.pose.orientation.x = qx
req.ik_request.pose_stamped.pose.orientation.y = qy
req.ik_request.pose_stamped.pose.orientation.z = qz
req.ik_request.pose_stamped.pose.orientation.w = qw
req.ik_request.timeout = rospy.Duration(0.05)
try:
serviceCallLink = rospy.ServiceProxy('/compute_ik', GetPositionIK)
response = serviceCallLink(req)
# print response
if response.error_code.val is not 1: # if nost success then issue
print "No solution at %0.2f, %0.2f, %0.2f" % (x, y, z)
else:
return True
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
def find_IK_solution(ik, target, seed, group_name):
response = ik(GetPositionIKRequest(ik_request = PositionIKRequest( group_name = group_name,
pose_stamped = PoseStamped( header = Header(frame_id="base_link"),
pose = target),
robot_state = RobotState(joint_state=seed))
) )
return response
def make_single_move(pose):
print "ok"
s = pose
print s
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "ar_marker_3"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = s[0]
request.ik_request.pose_stamped.pose.position.y = s[1]
request.ik_request.pose_stamped.pose.position.z = s[2]
request.ik_request.pose_stamped.pose.orientation.x = s[3]
request.ik_request.pose_stamped.pose.orientation.y = s[4]
request.ik_request.pose_stamped.pose.orientation.z = s[5]
request.ik_request.pose_stamped.pose.orientation.w = s[6]
try:
print "ok2"
response = compute_ik(request)
group = MoveGroupCommander("left_arm")
print "ok3"
group.set_pose_target(request.ik_request.pose_stamped)
print "ok4"
group.go()
print "in"
print x
#print "out"
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def get_position_ik(group_name,
pose_stamped,
ik_link_name=None,
robot_state=None):
"""
"""
# Use current robot state if None
if robot_state is None:
robot_state = get_robot_state()
# Prepare request
req = GetPositionIKRequest()
req.ik_request.group_name = group_name
if ik_link_name is not None:
req.ik_request.ik_link_name = ik_link_name
req.ik_request.pose_stamped = pose_stamped
req.ik_request.robot_state = robot_state
req.ik_request.avoid_collisions = True
# Call IK service
ik_srv = rospy.ServiceProxy('compute_ik', GetPositionIK)
try:
res = ik_srv(req)
except rospy.service.ServiceException, e:
rospy.logerr("Service 'compute_ik' call failed: %s", str(e))
return None
def requestIK(link_name, group_name, p, o, compute_ik):
px, py, pz = p
ox, oy, oz, ow = o
request = GetPositionIKRequest()
request.ik_request.group_name = group_name
request.ik_request.ik_link_name = link_name
request.ik_request.attempts = 10000
request.ik_request.pose_stamped.header.frame_id = "head_camera"
request.ik_request.pose_stamped.pose.position.x = px
request.ik_request.pose_stamped.pose.position.y = py
request.ik_request.pose_stamped.pose.position.z = pz
request.ik_request.pose_stamped.pose.orientation.x = ox
request.ik_request.pose_stamped.pose.orientation.y = oy
request.ik_request.pose_stamped.pose.orientation.z = oz
request.ik_request.pose_stamped.pose.orientation.w = ow
try:
# print request
print 'finding ik....'
response = compute_ik(request)
print 'moveIt succeeded'
# print(response)
group = MoveGroupCommander(group_name)
group.set_pose_target(request.ik_request.pose_stamped)
group.go()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def move_action(x):
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
step = 0
pose = [x]
if step == 0:
s = pose[step]
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "ar_marker_0"
request.ik_request.pose_stamped.pose.position.x = s[0]
request.ik_request.pose_stamped.pose.position.y = s[1]
request.ik_request.pose_stamped.pose.position.z = s[2]
request.ik_request.pose_stamped.pose.orientation.x = s[3]
request.ik_request.pose_stamped.pose.orientation.y = s[4]
request.ik_request.pose_stamped.pose.orientation.z = s[5]
request.ik_request.pose_stamped.pose.orientation.w = s[6]
response = compute_ik(request)
group = MoveGroupCommander("left_arm")
group.set_pose_target(request.ik_request.pose_stamped)
group.go()
step = step + 1
def ik_solve(self, pose, feed_joint_state=[0.0, 0.0, 0.0, 0.0, 0.0]):
"""
Given a end-effector pose, use inverse kinematics to determine the
nessecary joint angles to reach the pose.
CRED: https://github.com/uts-magic-lab/moveit_python_tools/blob/master/src/moveit_python_tools/get_ik.py
"""
# Build the the service request
req = GetPositionIKRequest()
req.ik_request.group_name = self.planning_group
req.ik_request.robot_state.joint_state.name = self.joint_names
req.ik_request.robot_state.joint_state.position = feed_joint_state
req.ik_request.avoid_collisions = True
req.ik_request.ik_link_name = self.eef_link
req.ik_request.pose_stamped = pose
req.ik_request.timeout.secs = 1.0
req.ik_request.timeout.nsecs = 0.0
req.ik_request.attempts = 0
# Try to send the request to the 'compute_ik' service
try:
resp = self.ik_srv.call(req)
return resp
except rospy.ServiceException:
rospy.logerr("Service execption: " + str(rospy.ServiceException))
def compute_ik(self, pose_stamped, timeout=rospy.Duration(5), verbose=True):
"""Computes inverse kinematics for the given pose.
Note: if you are interested in returning the IK solutions, we have
shown how to access them.
Args:
pose_stamped: geometry_msgs/PoseStamped.
timeout: rospy.Duration. How long to wait before giving up on the
IK solution.
Returns: True if the inverse kinematics were found, False otherwise.
"""
request = GetPositionIKRequest()
request.ik_request.pose_stamped = pose_stamped
request.ik_request.group_name = 'arm'
request.ik_request.timeout = timeout
response = self._compute_ik(request)
error_str = moveit_error_string(response.error_code.val)
success = error_str == 'SUCCESS'
if not success:
return False
if verbose:
joint_state = response.solution.joint_state
for name, position in itertools.izip(joint_state.name, joint_state.position):
if name in ArmJoints.names():
rospy.loginfo('{}: {}'.format(name, position))
return True
def compute_inverse_kinematics(self, end_effector_pose):
'''
compute the inverse kinematics of the given end effector pose,return joint values,end_effector_pose should be a pose
'''
request = GetPositionIKRequest()
request.ik_request.group_name = self.group_name
request.ik_request.ik_link_name = "wrist_3_link"
request.ik_request.attempts = 1000
request.ik_request.pose_stamped.header.frame_id = "first_link"
request.ik_request.pose_stamped.pose.position.x = end_effector_pose.position.x
request.ik_request.pose_stamped.pose.position.y = end_effector_pose.position.y
request.ik_request.pose_stamped.pose.position.z = end_effector_pose.position.z
request.ik_request.pose_stamped.pose.orientation.x = end_effector_pose.orientation.x
request.ik_request.pose_stamped.pose.orientation.y = end_effector_pose.orientation.y
request.ik_request.pose_stamped.pose.orientation.z = end_effector_pose.orientation.z
request.ik_request.pose_stamped.pose.orientation.w = end_effector_pose.orientation.w
#print(request)
ik_response = self.compute_ik(request)
#print(ik_response)
joint_value = ik_response.solution.joint_state.position
joint_values = []
if len(joint_value) < 6:
#rospy.logerr('the given end_effector_pose has no results')
return joint_values
else:
for i in range(len(joint_value)):
joint_values.append(joint_value[i])
#print(joint_value)
return joint_values
def main():
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
while not rospy.is_shutdown():
## get the (x,y,z) from the user
while True:
position = raw_input(
'Press enter to compute an IK solution:(x,y,z)')
position = position.split()
if len(position) != 3:
print "error: Please enter 3 floats"
else:
position = [float(i) for i in position]
break
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = position[0]
request.ik_request.pose_stamped.pose.position.y = position[1]
request.ik_request.pose_stamped.pose.position.z = position[2]
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
response = compute_ik(request)
#Print the response HERE
#print(response)
#print(response.error_code)
#print(response.solution)
error_code = response.error_code.val
if error_code != 1:
print("Get solution error with code %d\n" % ((error_code), ))
else:
print("position input (%f,%f,%f)" % tuple(position))
print("Solution get:")
joint_state = response.solution.joint_state
#print(joint_state)
print("Result by forward_kinematics")
lab3_helper.forward_kinematics(joint_state)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def main():
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node("ik_moveit")
pub = rospy.Publisher('/arm_controller/follow_joint_trajectory/goal',
FollowJointTrajectoryActionGoal,
queue_size=1)
compute_ik_srv = rospy.ServiceProxy("/compute_ik", GetPositionIK)
rospy.wait_for_service("/compute_ik")
arm_move_group = moveit_commander.MoveGroupCommander("manipulator")
robot_commander = moveit_commander.RobotCommander()
# compute reference position of the gripper ( link)
ref_pose = pq_to_pose([0.256, 0.294, 0.365], [0.707, 0.707, 0.0, 0.0])
rate = rospy.Rate(10.0)
iter_no = 0
max_iter = 10
while not rospy.is_shutdown():
# set reference position of the gripper ( link)
pose_ik = PoseStamped()
pose_ik.header.frame_id = "base_link"
pose_ik.header.stamp = rospy.Time.now()
pose_ik.pose = ref_pose
req = GetPositionIKRequest()
req.ik_request.group_name = "manipulator"
req.ik_request.robot_state = robot_commander.get_current_state()
req.ik_request.avoid_collisions = True
req.ik_request.ik_link_name = arm_move_group.get_end_effector_link()
req.ik_request.pose_stamped = pose_ik
# print(arm_move_group.get_end_effector_link())
ik_response = compute_ik_srv(req)
if ik_response.error_code.val == 1:
print('Goal state:')
print(ik_response.solution.joint_state.position)
goal_pose = FollowJointTrajectoryActionGoal()
# print (arm_move_group.get_joints())
goal_pose.goal.trajectory.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
goal_pose.goal.trajectory.points.append(JointTrajectoryPoint())
goal_pose.goal.trajectory.points[
0].positions = ik_response.solution.joint_state.position
goal_pose.goal.trajectory.points[0].velocities = [0, 0, 0, 0, 0, 0]
goal_pose.goal.trajectory.points[
0].time_from_start = rospy.Duration.from_sec(1.0)
pub.publish(goal_pose)
else:
print('Could not find solution to inverse kinematic')
rate.sleep()
iter_no = iter_no + 1
if iter_no > max_iter:
break
def _do_ik(self, px, py, pz, arm):
"""
Does inverse kinematics for a given arm and given position.
This solver is collision aware.
Args:
px, py, pz (float): Cartesian coordinates for position of arm end
effector.
arm (str): Which arm ("right_arm" or "left_arm") to do ik for.
Returns:
list: List of joint configuration for specific arm. Returns an
empty list if unable to do IK.
"""
IK_INFO_NAME = "/pr2_%s_kinematics/get_ik_solver_info" % (arm)
IK_NAME = "/compute_ik"
ik_solver_info_service_proxy = rospy.ServiceProxy(IK_INFO_NAME, GetKinematicSolverInfo)
ik_info_req = GetKinematicSolverInfoRequest()
rospy.loginfo("LightningTest: do_ik: Waiting for %s" % (IK_INFO_NAME))
rospy.wait_for_service(IK_INFO_NAME)
rospy.loginfo("About to get IK info.")
ik_info_res = ik_solver_info_service_proxy(ik_info_req)
rospy.loginfo("Got IK Solver info.")
ik_solver_service_proxy = rospy.ServiceProxy(IK_NAME, GetPositionIK)
ik_solve_req = GetPositionIKRequest()
ik_solve_req.ik_request.group_name = arm
ik_solve_req.ik_request.timeout = rospy.Duration(5.0)
ik_solve_req.ik_request.ik_link_name = "%s_wrist_roll_link" % (arm[0])
ik_solve_req.ik_request.pose_stamped.header.frame_id = "odom_combined"
ik_solve_req.ik_request.pose_stamped.pose.position.x = px
ik_solve_req.ik_request.pose_stamped.pose.position.y = py
ik_solve_req.ik_request.pose_stamped.pose.position.z = pz
ik_solve_req.ik_request.pose_stamped.pose.orientation.x = 0.0;
ik_solve_req.ik_request.pose_stamped.pose.orientation.y = 0.0;
ik_solve_req.ik_request.pose_stamped.pose.orientation.z = 0.0;
ik_solve_req.ik_request.pose_stamped.pose.orientation.w = 1.0;
# TODO: Consider retrieving current robto state.
ik_solve_req.ik_request.robot_state.joint_state.name = ik_info_res.kinematic_solver_info.joint_names;
for i in xrange(len(ik_info_res.kinematic_solver_info.joint_names)):
joint_limits = ik_info_res.kinematic_solver_info.limits[i]
ik_solve_req.ik_request.robot_state.joint_state.position.append((joint_limits.min_position + joint_limits.max_position)/2.0)
joint_constraint = JointConstraint()
joint_constraint.joint_name = ik_info_res.kinematic_solver_info.joint_names[i]
joint_constraint.position = (joint_limits.max_position + joint_limits.min_position) / 2.0
joint_constraint.tolerance_above = (joint_limits.max_position - joint_limits.min_position) / 2.0
joint_constraint.tolerance_below = joint_constraint.tolerance_above
ik_solve_req.ik_request.constraints.joint_constraints.append(joint_constraint)
rospy.loginfo("LightningTest: do_ik: Waiting for IK service.")
rospy.wait_for_service(IK_NAME)
ik_solve_res = ik_solver_service_proxy(ik_solve_req)
if ik_solve_res.error_code.val == ik_solve_res.error_code.SUCCESS:
return ik_solve_res.solution.joint_state.position[30:37]
else:
rospy.loginfo("Lightning tester: cannot move to sampled position...trying another position")
return []
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
self._move_group = userdata.move_group
self._move_group_prefix = userdata.move_group_prefix
self._tool_link = userdata.tool_link
self._offset = userdata.offset
self._rotation = userdata.rotation
self._srv_name = userdata.move_group_prefix + '/compute_ik'
self._ik_srv = ProxyServiceCaller({self._srv_name: GetPositionIK})
self._robot1_client = actionlib.SimpleActionClient(userdata.move_group_prefix + '/execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
self._robot1_client.wait_for_server()
rospy.loginfo('Execute Trajectory server is available for robot')
# Get transform between camera and robot1_base
while True:
rospy.sleep(0.1)
try:
target_pose = self._tf_buffer.transform(userdata.pose, "world")
break
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rospy.logerr("ComputeGraspState::on_enter - Failed to transform to world")
continue
# the grasp pose is defined as being located on top of the item
target_pose.pose.position.z += self._offset + 0.0
# rotate the object pose 180 degrees around - now works with -90???
#q_orig = [target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]
q_orig = [0, 0, 0, 1]
#q_rot = quaternion_from_euler(self._rotation, 0, 0)
q_rot = quaternion_from_euler(self._rotation, math.pi/2.0, 0) # math.pi/2.0 added by gerard!!
#q_rot = quaternion_from_euler(math.pi/-2.0, 0, 0)
res_q = quaternion_multiply(q_rot, q_orig)
target_pose.pose.orientation = geometry_msgs.msg.Quaternion(*res_q)
# use ik service to compute joint_values
self._srv_req = GetPositionIKRequest()
self._srv_req.ik_request.group_name = self._move_group
self._srv_req.ik_request.robot_state.joint_state = rospy.wait_for_message(self._move_group_prefix + '/joint_states', sensor_msgs.msg.JointState)
self._srv_req.ik_request.ik_link_name = self._tool_link # TODO: this needs to be a parameter
self._srv_req.ik_request.pose_stamped = target_pose
self._srv_req.ik_request.avoid_collisions = True
self._srv_req.ik_request.attempts = 500
try:
self._srv_result = self._ik_srv.call(self._srv_name, self._srv_req)
self._failed = False
except Exception as e:
Logger.logwarn('Could not call IK: ' + str(e))
self._failed = True
def check_ee_valid_pose(self,action):
#checking the validity of the end effector pose
#converting to joint state using ik and then getting the validity
GPIK_Request =GetPositionIKRequest()
GPIK_Request.group_name = GPIK_Request.group_name = 'arm_controller'
GPIK_Request.robot_state.joint_state = self.joints
GPIK_Request.avoid_collisions = True
# this poses is related to the reference frame of gazebo
# the pose is set as a radian value between 1.571 and -1.571
GPIK_Request.pose_stamped.pose.position.x = action[ 0 ]
GPIK_Request.pose_stamped.pose.position.y = action[ 1 ]
GPIK_Request.pose_stamped.pose.position.z = action[ 2 ]
GPIK_Request.pose_stamped.pose.orientation = quaternion_from_euler([0, 1.571, action[3]])
GPIK_Response = self.joint_state_from_pose_client(GPIK_Request)
if GPIK_Response.error_code == 1:
return True
else:
return GPIK_Response.error_code
def get_position_ik(robot_state,
ik_link_name,
pose_stamped,
constraints=Constraints(),
avoid_collisions=True,
timeout=rospy.Time(1),
persistent=False,
wait_timeout=None):
"""Call position IK service.
INPUT (see moveit_msgs/PositionIKRequest for details)
robot_state [moveit_msgs/RobotState]
ik_link_name [String, default=""]
pose_stamped [geometry_msgs/PoseStamped, default=None]
constraints [moveit_msgs/Constraints, default=None]
avoid_collisions [Bool, default=True]
timeout [Float, default=1.0]
persistent [Bool, default=False]
Set to True if calling this function many times.
wait_timeout [float, default=None]
Time to wait for the service.
OUTPUT
response [moveit_msgs/GetPositionIKResponse]
"""
ik_srv = get_service_proxy(SRV_GET_POSITION_IK_SERVICE, GetPositionIK,
persistent, wait_timeout)
# Make a copy
pose_stamped = deepcopy(pose_stamped)
# Fix the link if differ from default
new_link_name, new_pose = utils.fix_link_for_ik(pose_stamped.pose,
ik_link_name)
if not new_link_name == ik_link_name:
rospy.logdebug("Link %s is not IK link, was converted to %s",
ik_link_name, new_link_name)
ik_link_name = new_link_name
pose_stamped.pose = new_pose
req = GetPositionIKRequest()
req.ik_request.group_name = infer_group_from_ee(ik_link_name)
req.ik_request.robot_state = robot_state
req.ik_request.constraints = constraints
req.ik_request.avoid_collisions = avoid_collisions
req.ik_request.ik_link_name = ik_link_name
req.ik_request.pose_stamped = pose_stamped
req.ik_request.ik_link_names = []
req.ik_request.pose_stamped_vector = []
req.ik_request.timeout = timeout
req.ik_request.attempts = 1
res = ik_srv(req)
return res
def main(x, y, z):
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
while not rospy.is_shutdown():
raw_input('Press enter to compute an IK solution:')
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = x
request.ik_request.pose_stamped.pose.position.y = y
request.ik_request.pose_stamped.pose.position.z = z
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
response = compute_ik(request)
message = response.solution.joint_state
####
#Print the contents of the message to the console
theta = []
theta.append(message.position[1])
theta.append(message.position[2])
theta.append(message.position[3])
theta.append(message.position[4])
theta.append(message.position[5])
theta.append(message.position[6])
theta.append(message.position[7])
print("theta = ")
print(theta)
print("\n")
print("------")
print("gst = ")
print(lab3.lab3(theta))
print("--------------")
print(response)
#Print the response HERE
####
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def main(robo):
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
arm = 'left'
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
if robo == 'sawyer':
arm = 'right'
while not rospy.is_shutdown():
raw_input('Press [ Enter ]: ')
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = arm + "_arm"
#Alan does not have a gripper so replace link with 'right_wrist' instead
link = arm + "_gripper"
if robo == 'sawyer':
link += '_tip'
request.ik_request.ik_link_name = link
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = 0.5
request.ik_request.pose_stamped.pose.position.y = 0.5
request.ik_request.pose_stamped.pose.position.z = 0.0
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
response = compute_ik(request)
#Print the response HERE
print(response)
group = MoveGroupCommander(arm + "_arm")
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# TRY THIS
# Setting just the position without specifying the orientation
###group.set_position_target([0.5, 0.5, 0.0])
# Plan IK and execute
group.go()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def get_ik(self,
pose_stamped,
group=None,
ik_timeout=None,
ik_attempts=None,
avoid_collisions=None):
"""
Do an IK call to pose_stamped pose. geometry_msgs/PoseStamped
pose_stamped: The 3D pose (with header.frame_id) to which
compute the IK.
Parameters
----------
pose_stamped : :obj:`geometry_msgs.msg.PoseStamped`
goal pose
group : string
The MoveIt! group.
ik_timeout : float
The timeout for the IK call.
ik_attemps : int
The maximum # of attemps for the IK.
avoid_collisions : bool
If to compute collision aware IK.
Returns
-------
moveit_msgs.srv.GetPositionIKResponse
Response from /compute_ik service
"""
if group is None:
group = self.group_name
if ik_timeout is None:
ik_timeout = self.ik_timeout
if ik_attempts is None:
ik_attempts = self.ik_attempts
if avoid_collisions is None:
avoid_collisions = self.avoid_collisions
req = GetPositionIKRequest()
req.ik_request.group_name = group
req.ik_request.pose_stamped = pose_stamped
req.ik_request.timeout = rospy.Duration(ik_timeout)
req.ik_request.attempts = ik_attempts
req.ik_request.avoid_collisions = avoid_collisions
try:
resp = self.ik_srv.call(req)
return resp
except rospy.ServiceException as e:
rospy.logerr("Service exception: " + str(e))
resp = GetPositionIKResponse()
resp.error_code = 99999 # Failure
return resp
def on_enter(self, userdata):
# This method is called when the state becomes active, i.e. a transition from another state to this one is taken.
# It is primarily used to start actions which are associated with this state.
# Get transform between camera and robot1_base
while True:
rospy.sleep(0.1)
try:
target_pose = self._tf_buffer.transform(userdata.pose, "world")
break
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logerr(
"ComputeGraspState::on_enter - Failed to transform to world"
)
continue
# the grasp pose is defined as being located on top of the box
target_pose.pose.position.z += self._offset + 0.0
# rotate the object pose 180 degrees around - now works with -90???
#q_orig = [target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]
q_orig = [0, 0, 0, 1]
q_rot = quaternion_from_euler(self._rotation, 0, 0)
#q_rot = quaternion_from_euler(math.pi/-2.0, 0, 0)
res_q = quaternion_multiply(q_rot, q_orig)
target_pose.pose.orientation = geometry_msgs.msg.Quaternion(*res_q)
# use ik service to compute joint_values
self._srv_req = GetPositionIKRequest()
self._srv_req.ik_request.group_name = self._group
self._srv_req.ik_request.robot_state.joint_state = rospy.wait_for_message(
self._group + '/joint_states', sensor_msgs.msg.JointState)
self._srv_req.ik_request.ik_link_name = self._tool_link # TODO: this needs to be a parameter
self._srv_req.ik_request.pose_stamped = target_pose
self._srv_req.ik_request.avoid_collisions = True
self._srv_req.ik_request.attempts = 500
for tries in range(0, 3):
try:
self._srv_result = self._ik_srv.call(self._srv_name,
self._srv_req)
self._failed = False
Logger.logwarn('ComputeGraspState: attempt ' + str(tries))
if int(self._srv_result.error_code.val) == 1:
break
except Exception as e:
Logger.logwarn('Could not call IK: ' + str(e))
self._failed = True
def main():
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
while not rospy.is_shutdown():
raw_input('Press [ Enter ]: ')
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_gripper"
request.ik_request.attempts = 20
#request.ik_request.pose_stamped.header.frame_id = "base"
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
euler_angle = [-np.pi, 0, 0]
traslation = [0.591, 0.261, 0.327 - 0.1]
quaternion = tf.transformations.quaternion_from_euler(
euler_angle[0], euler_angle[1], euler_angle[2])
request.ik_request.pose_stamped.pose.position.x = traslation[0]
request.ik_request.pose_stamped.pose.position.y = traslation[1]
request.ik_request.pose_stamped.pose.position.z = traslation[2]
request.ik_request.pose_stamped.pose.orientation.x = quaternion[0]
request.ik_request.pose_stamped.pose.orientation.y = quaternion[1]
request.ik_request.pose_stamped.pose.orientation.z = quaternion[2]
request.ik_request.pose_stamped.pose.orientation.w = quaternion[3]
try:
#Send the request to the service
response = compute_ik(request)
#Print the response HERE
#print(response)
group = MoveGroupCommander("left_arm")
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# TRY THIS
# Setting just the position without specifying the orientation
###group.set_position_target([0.5, 0.5, 0.0])
# Plan IK and execute
group.go()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def move_to(self,
pos_x,
pos_y,
pos_z,
orien_x,
orien_y,
orien_z,
orien_w,
ik,
orien_const=None):
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "right_arm"
request.ik_request.ik_link_name = "right_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = pos_x
request.ik_request.pose_stamped.pose.position.y = pos_y
request.ik_request.pose_stamped.pose.position.z = pos_z
request.ik_request.pose_stamped.pose.orientation.x = orien_x
request.ik_request.pose_stamped.pose.orientation.y = orien_y
request.ik_request.pose_stamped.pose.orientation.z = orien_z
request.ik_request.pose_stamped.pose.orientation.w = orien_w
try:
#Send the request to the service
response = ik(request)
#Print the response HERE
print(response)
group = MoveGroupCommander("right_arm")
if orien_const is not None:
constraints = Constraints()
constraints.orientation_constraints = orien_const
group.set_path_constraints(constraints)
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# TRY THIS
# Setting just the position without specifying the orientation
# group.set_position_target([0.5, 0.5, 0.0])
# Plan IK and execute
group.go()
except rospy.ServiceException, e:
print("Service call failed: %s")
def move_group_pick(self, compute_ik, transformation_mat):
t = geometry_msgs.msg.Pose()
position = tf.transformations.translation_from_matrix(
transformation_mat)
orientation = tf.transformations.quaternion_from_matrix(
transformation_mat)
t.position.x = position[0]
t.position.y = position[1]
t.position.z = position[2]
t.orientation.x = orientation[0]
t.orientation.y = orientation[1]
t.orientation.z = orientation[2]
t.orientation.w = orientation[3]
# With this function, we will be able t achieve the pre grasp position
# scene = PlanningSceneInterface()
# p = PoseStamped()
# start = rospy.get_time()
# seconds = rospy.get_time()
# box_name = "obj1"
# while (seconds - start < 5) and not rospy.is_shutdown():
# # Test if the box is in attached objects
# attached_objects = scene.get_attached_objects([box_name])
# is_attached = len(attached_objects.keys()) > 0
# is_known = box_name in scene.get_known_object_names()
# seconds = rospy.get_time()
self.group.set_named_target("initial")
self.group.go()
request = GetPositionIKRequest()
request.ik_request.group_name = "arm"
request.ik_request.ik_link_name = "wrist_3_link"
request.ik_request.attempts = 20
#request.ik_request.pose_stamped.header.frame_id = "base"
request.ik_request.pose_stamped.header.frame_id = "ur10_base_link"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = t.position.x - 0.2
request.ik_request.pose_stamped.pose.position.y = t.position.y
request.ik_request.pose_stamped.pose.position.z = t.position.z - 0.4
request.ik_request.pose_stamped.pose.orientation.x = -0.707
request.ik_request.pose_stamped.pose.orientation.y = 0
request.ik_request.pose_stamped.pose.orientation.z = 0
request.ik_request.pose_stamped.pose.orientation.w = 0.707
response = compute_ik(request)
group = self.group
group.set_pose_target(request.ik_request.pose_stamped)
group.go()
self.gripper.set_named_target("close")
self.gripper.go()
self.group.set_named_target("initial")
self.group.go()
def compute_moveit_ik(end_effector_pose, seed, group="right_arm"):
req = GetPositionIKRequest()
req.ik_request.group_name = group
arm = "l" if group == "left_arm" else "r"
if group == "left_arm":
req.ik_request.robot_state.joint_state.name = ['yumi_joint_%d_l' % i for i in [1, 2, 7, 3, 4, 5, 6]]
else:
req.ik_request.robot_state.joint_state.name = ['yumi_joint_%d_r' % i for i in [1, 2, 7, 3, 4, 5, 6]]
req.ik_request.robot_state.joint_state.position = seed
req.ik_request.pose_stamped = end_effector_pose
req.ik_request.timeout = rospy.Duration(0.05)
req.ik_request.attempts = 20
res = ik_srv(req).solution
return res.joint_state.position
def movement(des_coor, i):
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
received_info = True
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
while received_info == True:
#Construct the requests
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_gripper"
request.ik_request.attempts = 30
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
print(des_coor)
request.ik_request.pose_stamped.pose.position.x = des_coor[0]
request.ik_request.pose_stamped.pose.position.y = des_coor[1]
request.ik_request.pose_stamped.pose.position.z = 0.25 + initial_xyz[
2] # this should be a found constant
# Should be constant Determined by Experiment
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
response = compute_ik(request)
#Print the response HERE
print(response)
if response.error_code.val == 1:
received_info = False
else:
received_info = False
# received_info = False
group = MoveGroupCommander("left_arm")
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# Plan IK and execute
group.go()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def discard(self, poses):
validp = []
validrs = []
req = GetPositionIKRequest()
req.ik_request.group_name = self.arm_move_group_name
req.ik_request.robot_state = self.robot_commander.get_current_state()
req.ik_request.avoid_collisions = True
for p in poses:
req.ik_request.pose_stamped = p
k = self.compute_ik_srv(req)
if k.error_code.val == 1:
validp.append(p)
validrs.append(k.solution)
if validp:
return [validp, validrs]
return []
def get_position_ik(self, position, orientation):
rospy.wait_for_service(self.ns + 'solve_ik')
req = GetPositionIKRequest()
req.ik_request.pose_stamped.pose.position.x = position[0]
req.ik_request.pose_stamped.pose.position.y = position[1]
req.ik_request.pose_stamped.pose.position.z = position[2]
req.ik_request.pose_stamped.pose.orientation.x = orientation[0]
req.ik_request.pose_stamped.pose.orientation.y = orientation[1]
req.ik_request.pose_stamped.pose.orientation.z = orientation[2]
req.ik_request.pose_stamped.pose.orientation.w = orientation[3]
result = self._get_position_ik(req)
if result.error_code.val == result.error_code.SUCCESS:
return list(result.solution.joint_state.position)
else:
return []
try:
joint_states_client = rospy.ServiceProxy('return_joint_states', ReturnJointStates)
resp = joint_states_client()
current_joint_position = resp.position
except rospy.ServiceException, e:
print "error when calling return_joint_states: %s"%e
try:
query_client = rospy.ServiceProxy('pr2_right_arm_kinematics/get_ik_solver_info', GetKinematicSolverInfo)
resp = query_client()
joint_names = resp.kinematic_solver_info.joint_names
limits = resp.kinematic_solver_info.limits
except rospy.ServiceException, e:
print 'Cannot call ik solver query service'%e
try:
ik_client = rospy.ServiceProxy('/pr2_right_arm_kinematics/get_ik', GetPositionIK)
req = GetPositionIKRequest()
req.ik_request.ik_link_name = "r_wrist_roll_link"
req.ik_request.pose_stamped = pose_stamped
req.ik_request.ik_seed_state.joint_state.name = joint_names
req.ik_request.ik_seed_state.joint_state.position = current_joint_position
req.timeout = rospy.Duration(5.0)
resp = ik_client(req)
if resp.error_code.val == resp.error_code.SUCCESS:
print 'Found IK Solution:', resp.solution.joint_state.position
return (resp.solution.joint_state.position, True)
else:
print 'error code: ', resp.error_code.val
return ([], False)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
return ([], False)
