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 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 handle_solve_ik(self, req):
with self._lock:
#print("-----------request!")
pos = req.ik_request.pose_stamped.pose.position
ori = req.ik_request.pose_stamped.pose.orientation
group_name = req.ik_request.group_name
#print(pos)
#print(ori)
#print('group_name: {}'.format(group_name))
res = GetPositionIKResponse()
res.solution.joint_state.header = req.ik_request.pose_stamped.header
self.update_tool_offset(group_name)
joint = self._rb.solve_ik(pos, ori)
if len(joint) != 0:
#print("pose!")
#print(req.ik_request.pose_stamped.pose)
#print("req!")
#print(req.ik_request)
#print("joint!")
#print(joint)
#print("success!")
res.solution.joint_state.name = self.joint_names
res.solution.joint_state.position = joint
res.error_code.val = res.error_code.SUCCESS
print(res.solution.joint_state.position)
else:
res.error_code.val = res.error_code.NO_IK_SOLUTION
return res
def ik(self, pose_stamped=None, position=None, orientation=None):
'''
poisiton = [x,y,z], orientation = [i,j,k,w]
'''
if pose_stamped is not None:
ps = pose_stamped
elif position is not None and orientation is not None:
ps = PoseStamped()
ps.header.time = rospy.get_rostime()
ps.header.frame_id = "world"
ps.pose.position.x = position[0]
ps.pose.position.y = position[1]
ps.pose.position.z = position[2]
ps.pose.orientation.x = orientation[0]
ps.pose.orientation.y = orientation[1]
ps.pose.orientation.z = orientation[2]
ps.pose.orientation.w = orientation[3]
else:
raise ValueError(
"need to provide at either pose_stamped or position and orientation"
)
req = GetPositionIKRequest()
req.ik_request.group_name = "manipulator"
req.ik_request.robot_state = self.get_robot_state()
req.ik_request.pose_stamped = ps
req.ik_request.ik_link_name = self.ee_link
req.ik_request.timeout = rospy.Duration(1.0)
req.ik_request.attempts = 1
req.ik_request.avoid_collisions = False
try:
resp = self.moveit_ik.call(req)
joint_names = resp.solution.joint_state.name
joint_values = resp.solution.joint_state.position
return joint_values
except rospy.ServiceException as e:
rospy.logerr("Service exception: " + str(e))
resp = GetPositionIKResponse()
resp.error_code = 99999 # Failure
return resp
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.
:param geometry_msgs/PoseStamped pose_stamped: The 3D pose
(with header.frame_id)
to which compute the IK.
:param str group: The MoveIt! group.
:param float ik_timeout: The timeout for the IK call.
:param int ik_attemps: The maximum # of attemps for the IK.
:param bool avoid_collisions: If to compute collision aware IK.
"""
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 handle_solve_ik(self, req):
print "-----------request!"
pos = req.ik_request.pose_stamped.pose.position
ori = req.ik_request.pose_stamped.pose.orientation
group_name = req.ik_request.group_name
tool = self._config[group_name]['tool']
res = GetPositionIKResponse()
print pos
print ori
rb = self._config[group_name]['robot']
if tool:
print('tool_offset: {}'.format(tool.get_offset()))
offset = tool.get_offset()
if self._last_offset != offset:
rb.set_tool(tool)
self._last_offset = offset
else:
if self._last_offset != []:
rb.set_tool(tool)
self._last_offset = []
joint = rb.get_position_ik([pos.x, pos.y, pos.z],
[ori.x, ori.y, ori.z, ori.w])
res.solution.joint_state.header = req.ik_request.pose_stamped.header
if len(joint) != 0:
print "pose!"
print req.ik_request.pose_stamped.pose
print "req!"
print req.ik_request
print "joint!"
print joint
print "success!"
#res.solution.joint_state.name = self._config[group_name]['joint'] + ['left_ezgripper_knuckle_palm_L1_1', 'left_ezgripper_knuckle_L1_L2_1', 'left_ezgripper_knuckle_palm_L1_2', 'left_ezgripper_knuckle_L1_L2_2']
res.solution.joint_state.name = self._config[group_name]['joint']
#res.solution.joint_state.position = joint + [0, 0, 0, 0]
res.solution.joint_state.position = joint
res.error_code.val = res.error_code.SUCCESS
#res.error_code.val = 10
print '>--------------res'
print res
print '<--------------res'
print res.solution.joint_state.position
else:
print('NO IK!')
res.error_code.val = res.error_code.NO_IK_SOLUTION
return res