def get_inverse_kinematics(self, pose):
try:
rospy.wait_for_service('compute_ik', 2)
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.logerr("Arm commander - Impossible to connect to IK service : " + str(e))
return False, []
try:
tool_link_pose = self.__transform_handler.tcp_to_ee_link_pose_target(pose, "tool_link")
moveit_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
req = PositionIKRequest()
req.group_name = self.__arm.get_name()
req.ik_link_name = self.__arm.get_end_effector_link()
req.robot_state.joint_state.name = self.__arm_state.joints_name
req.robot_state.joint_state.position = self.__arm_state.joint_states
req.pose_stamped.pose = tool_link_pose
response = moveit_ik(req)
except rospy.ServiceException as e:
rospy.logerr("Arm commander - Service call failed: {}".format(e))
return False, []
if response.error_code.val == MoveItErrorCodes.NO_IK_SOLUTION:
rospy.logerr_throttle(0.5, "Arm commander - MoveIt didn't find an IK solution")
return False, []
elif response.error_code.val != MoveItErrorCodes.SUCCESS:
rospy.logerr("Arm commander - IK Failed : code error {}".format(response.error_code.val))
return False, []
return True, list(response.solution.joint_state.position[:6])
def get_position_ik(self, target_pose):
"""
Return a list of joints position (solution) from a PoseStamped (target_pose), using the /compute_ik service
"""
try:
rospy.wait_for_service('/compute_ik', 2)
except (rospy.ServiceException, rospy.ROSException) as e:
rospy.logerr(
"Arm commander - Impossible to connect to IK service : " +
str(e))
return [], False
service = rospy.ServiceProxy('/compute_ik', GetPositionIK)
get_pose_ik = PositionIKRequest()
get_pose_ik.group_name = "arm"
get_pose_ik.ik_link_name = "tool_link"
get_pose_ik.robot_state.joint_state.name = [
"joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6"
]
get_pose_ik.robot_state.joint_state.position = self.current_joint.position
get_pose_ik.pose_stamped.pose = target_pose.pose
response = service(get_pose_ik)
solution = []
if response.error_code.val == response.error_code.SUCCESS:
for i in range(6):
solution.append(response.solution.joint_state.position[i])
result = True
else:
result = False
return solution, result
def computeIK(self, pose):
# Create a pose to compute IK for
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = 'base_link' # Hard coded for now
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.pose = pose
# Create a moveit ik request
ik_request = PositionIKRequest()
ik_request.group_name = 'arm' # Hard coded for now
ik_request.pose_stamped = pose_stamped
ik_request.timeout.secs = 0.1
ik_request.avoid_collisions = True
try:
request_value = self.compute_ik(ik_request)
if request_value.error_code.val == -31:
rospy.logwarn("Teleop Arm: No IK Solution")
if request_value.error_code.val == 1:
joint_positions = request_value.solution.joint_state.position[1:7]
joint_names = request_value.solution.joint_state.name[1:7]
return joint_positions,joint_names
else:
return None,None
except rospy.ServiceException, e:
print "IK service request failed: %s" % e
return None,None
def computeIK2(self, pose):
# Create a pose to compute IK for
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = 'base_link' # Hard coded for now
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.pose = pose
# Create a moveit ik request
ik_request = PositionIKRequest()
ik_request.group_name = 'left_arm'
ik_request.pose_stamped = pose_stamped
ik_request.timeout.secs = 0.1
ik_request.avoid_collisions = True
try:
request_value = self.compute_ik(ik_request)
if request_value.error_code.val == -31:
rospy.logwarn("Teleop Arm: No IK Solution")
if request_value.error_code.val == 1:
joint_positions = request_value.solution.joint_state.position[
1:7]
joint_names = request_value.solution.joint_state.name[1:7]
return joint_positions, joint_names
else:
return None, None
except rospy.ServiceException, e:
print "IK service request failed: %s" % e
return None, None
def on_traj(traj):
start_pose = group.get_current_pose()
missed = 0
for p in traj.points:
target_pose = deepcopy(start_pose);
target_pose.pose.position.z+=0.1#p.transforms[0].translation.x
#target_pose.pose.position.y+=p.transforms[0].translation.y
service_request = PositionIKRequest()
service_request.group_name = group.get_name()
#service_request.ik_link_name = ENDEFFECTOR
service_request.pose_stamped = target_pose
#service_request.timeout.secs= 0.005
service_request.avoid_collisions = True
service_request.robot_state = robot.get_current_state()
pdb.set_trace()
group.set_joint_value_target(target_pose,True)
plan = group.plan()
goal = JointTrajectoryGoal();
goal.trajectory = deepcopy(plan.joint_trajectory)
goal.trajectory.points = deepcopy([plan.joint_trajectory.points[9]]) #just take the last point for now
trajectory_client.send_goal(goal);
def ik(self, pose_stamped, group_name='arm'):
""" Computes the inverse kinematics """
req = PositionIKRequest()
req.group_name = group_name
req.pose_stamped = pose_stamped
req.timeout.secs = 0.1
req.avoid_collisions = False
try:
res = self.compute_ik(req)
return res
except rospy.ServiceException, e:
print("IK service call failed: {}".format(e))
def setTargetPoseSequenceMoveIt(limb, pos_list, rpy_list, tm_list):
'''
Create a array of limb joint angles for the each waypoints
from data of the end effector postions and postures
using MoveIt! IK for computing joint angles.
@type limb : str
@param limb : limb to create a pattern, right arm 'rarm' or left arm 'larm'
@type pos_list : [[float,float,float],[float,float,float],...]
@param pos_list : Array of end effector positions (x,y,z) [m]
@type rpy_list : [[float,float,float],[float,float,float],...]
@param rpy_list : Array of end effector postures (r,p,y) [m]
@type tm_list : [float,float,...]
@param tm_list : Array of motion time lengths of the each pose [s]
@rtype : [[float,float,...],[float,float,...],...]
@return : Array of limb joint angles [rad]
'''
pattern_arm = []
wpose = geometry_msgs.msg.Pose()
rospy.wait_for_service('compute_ik')
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
if limb == 'rarm':
limb_name = "right_arm" # Limb 'right_arm', 'left_arm'
else:
limb_name = "left_arm"
joint_name = filter(lambda n:n[0] == limb, robot.Groups)[0][1]
for pos, rpy, tm in zip(pos_list, rpy_list, tm_list):
quaternion = tf.transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2])
wpose.position.x = pos[0]
wpose.position.y = pos[1]
wpose.position.z = pos[2]
wpose.orientation.x = quaternion[0]
wpose.orientation.y = quaternion[1]
wpose.orientation.z = quaternion[2]
wpose.orientation.w = quaternion[3]
req = PositionIKRequest()
req.group_name = limb_name
#req.ik_link_name = ''
req.pose_stamped.pose = wpose
ret = compute_ik(req)
if ret.error_code.val != 1:
error(ret)
pattern_arm.append(map(lambda n: n[1],filter(lambda n:n[0] in joint_name, zip(ret.solution.joint_state.name, ret.solution.joint_state.position))))
return pattern_arm
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 get_ik(self, pose_stamped, joint_seed=None):
"""
Computes the inverse kinematics
returns a list of joint angles
if joint_seed is not specified, it will use the robot's current position
"""
robot_state = self.robot.get_current_state()
if joint_seed is not None:
robot_state.joint_state.position = joint_seed
req = PositionIKRequest()
req.group_name = self.group_name
req.robot_state = self.robot.get_current_state()
req.avoid_collisions = True
req.ik_link_name = self.group.get_end_effector_link()
req.pose_stamped = pose_stamped
try:
res = self.ik_solver(req)
return res.solution.joint_state.position
except rospy.ServiceException, e:
print("IK service call failed: {}".format(e))
def get_ik(target, group = "right_arm_and_torso"):
"""
:param target: a PoseStamped give the desired position of the endeffector.
"""
service_request = PositionIKRequest()
service_request.group_name = group
service_request.ik_link_name = ENDEFFECTOR
service_request.pose_stamped = target
service_request.timeout.secs= 0.005
service_request.avoid_collisions = False
try:
resp = compute_ik(ik_request = service_request)
return resp
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def get_ik(target, group="right_arm_and_torso"):
"""
:param target: a PoseStamped give the desired position of the endeffector.
"""
service_request = PositionIKRequest()
service_request.group_name = group
#service_request.robot_state = initial_state
#service_request.ik_link_name = "pen_link"
service_request.pose_stamped = target
service_request.timeout.secs = 0.1
service_request.avoid_collisions = False
try:
resp = compute_ik(ik_request=service_request)
return resp
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def get_ik(self, target_pose, avoid_collisions=False, joint_states=None):
"""
Computes the inverse kinematics for a given pose. It returns a JointState
@param target_pose - A given pose of type PoseStamped
@param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false
"""
service_request = PositionIKRequest()
service_request.group_name = self._name
service_request.ik_link_name = self._move_group_commander.get_end_effector_link()
service_request.pose_stamped = target_pose
service_request.timeout.secs = 0.5
service_request.avoid_collisions = avoid_collisions
if joint_states is None:
service_request.robot_state.joint_state = self.get_joints_state()
else:
service_request.robot_state.joint_state = joint_states
try:
resp = self._compute_ik(ik_request=service_request)
# Check if error_code.val is SUCCESS=1
if resp.error_code.val != 1:
if resp.error_code.val == -10:
rospy.logerr("Unreachable point: Start state in collision")
elif resp.error_code.val == -12:
rospy.logerr("Unreachable point: Goal state in collision")
elif resp.error_code.val == -31:
rospy.logerr("Unreachable point: No IK solution")
else:
rospy.logerr("Unreachable point (error: %s)" % resp.error_code)
return
else:
return resp.solution.joint_state
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
def get_ik(target):
"""
:param target: a PoseStamped give the desired position of the endeffector.
"""
pose = group.get_current_pose(group.get_end_effector_link())
constraints = Constraints()
orientation_constraint = OrientationConstraint()
orientation_constraint.header = pose.header
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.orientation = pose.pose.orientation
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 2*pi
current_orientation_list = [pose.pose.orientation.x,
pose.pose.orientation.y,
pose.pose.orientation.z,
pose.pose.orientation.w]
# get euler angle from quaternion
(roll, pitch, yaw) = euler_from_quaternion(current_orientation_list)
pitch = pi
roll = 0
orientation_constraint.weight = 1
[orientation_constraint.orientation.x, orientation_constraint.orientation.y, orientation_constraint.orientation.z, orientation_constraint.orientation.w] = \
quaternion_from_euler(roll, pitch, yaw)
constraints.orientation_constraints.append(orientation_constraint)
# group.set_path_constraints(constraints)
#####################################################################
rospy.wait_for_service('compute_ik')
request_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
service_request = PositionIKRequest()
service_request.group_name = 'robot'
service_request.pose_stamped.header.frame_id = 'base_footprint'
# service_request.pose_stamped = group.get_current_pose()
service_request.robot_state = robot.get_current_state()
service_request.ik_link_name = 'arm_link_5'
# Set position
service_request.pose_stamped.pose.position.x = target[0]
service_request.pose_stamped.pose.position.y = target[1]
service_request.pose_stamped.pose.position.z = target[2]
service_request.pose_stamped.pose.orientation.w =1
service_request.constraints.orientation_constraints.append(orientation_constraint)
service_request.timeout.secs= 4
service_request.attempts= 2
service_request.avoid_collisions = True
resp = request_ik(service_request)
return resp
def computeIK(self, pose):
# Create a pose to compute IK for
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = 'base_link' # Hard coded for now
pose_stamped.header.stamp = rospy.Time.now()
pose_stamped.pose = pose
# Create a moveit ik request
ik_request = PositionIKRequest()
ik_request.group_name = 'manipulator' # Hard coded for now
ik_request.pose_stamped = pose_stamped
ik_request.timeout.secs = 1
#ik_request.avoid_collisions = True
ik_request.robot_state = robot.get_current_state()
ik_request.robot_state.is_diff = True
#ik_request.avoid_collisions = True
#ik_request.ik_link_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
try:
print(ik_request)
request_value = self.compute_ik.call(ik_request)
if request_value.error_code.val == -31:
rospy.logwarn("No IK Solution")
if request_value.error_code.val == 1:
joint_positions = request_value.solution.joint_state.position[1:7]
joint_names = request_value.solution.joint_state.name[1:7]
return joint_positions,joint_names
else:
return None,None
except rospy.ServiceException, e:
print "IK service request failed: %s" % e
return None,None
def _plan_linear_motion(self,
start_joint_state,
waypoints,
ik_request=None):
# Define joint constraint builder function
def build_joint_constraints(joint_state):
joint_constraints = []
for i, joint_angle in enumerate(joint_state):
jc = JointConstraint()
jc.joint_name = self.JOINTS[i]
jc.position = joint_angle
jc.tolerance_above = np.radians(
self.MAX_LINEAR_JOINT_FLUCTUATION)
jc.tolerance_below = np.radians(
self.MAX_LINEAR_JOINT_FLUCTUATION)
jc.weight = 1.0
joint_constraints.append(jc)
return joint_constraints
# Build IK request message
if ik_request is None:
# Build robot state message
robot_state = self._moveit_robot.get_current_state()
robot_state.attached_collision_objects = self._moveit_scene.get_attached_objects(
).values()
robot_state.joint_state.velocity = []
robot_state.joint_state.effort = []
robot_state.joint_state.position = [0, 0, 0, 0, 0, 0]
# Build IK request message
ik_request = PositionIKRequest()
ik_request.group_name = "manipulator"
ik_request.robot_state = robot_state
ik_request.avoid_collisions = True
ik_request.pose_stamped.header.frame_id = 'world'
ik_request.timeout = rospy.Duration(secs=self.PLAN_TIMEOUT)
motion_path, finished_chain = [start_joint_state], True
ik_request.constraints.joint_constraints = build_joint_constraints(
start_joint_state)
for waypoint in waypoints:
ik_request.pose_stamped.pose = waypoint
ik_request.robot_state.joint_state.position[0:6] = motion_path[-1]
ik_res = self._compute_ik(ik_request)
if ik_res.error_code.val != 1:
print(
f'Couldn\'t finish linear chain ({ik_res.error_code.val})')
finished_chain = False
break
# Update constrains for next waypoint
joint_values = ik_res.solution.joint_state.position
ik_request.constraints.joint_constraints = build_joint_constraints(
joint_values)
motion_path.append(list(joint_values))
if not finished_chain: return False, None
else: return True, motion_path[1:]
def compute_ik(self, pose):
if isinstance(pose, rox.Transform):
pose = rox_msg.transform2pose_stamped_msg(pose)
assert isinstance(pose, PoseStamped)
if pose.header.frame_id is None or pose.header.frame_id == "":
pose.header.frame_id = "world"
get_planning_scene_req = GetPlanningSceneRequest()
get_planning_scene_req.components.components = PlanningSceneComponents.ROBOT_STATE
get_planning_scene_res = self._get_planning_scene(
get_planning_scene_req)
robot_state_start = get_planning_scene_res.scene.robot_state
ik_request = PositionIKRequest()
ik_request.group_name = self.moveit_group.get_name()
ik_request.robot_state = robot_state_start
ik_request.avoid_collisions = True
ik_request.timeout = rospy.Duration(30.0)
ik_request.pose_stamped = pose
ik_request.ik_link_name = self.moveit_group.get_end_effector_link()
for i in xrange(len(robot_state_start.joint_state.name)):
j = JointConstraint()
j.joint_name = robot_state_start.joint_state.name[i]
j.position = robot_state_start.joint_state.position[i]
j.tolerance_above = np.deg2rad(170)
j.tolerance_below = np.deg2rad(170)
j.weight = 100
ik_request.constraints.joint_constraints.append(j)
ik_request_req = GetPositionIKRequest(ik_request)
ik_request_res = self._compute_ik(ik_request_req)
if (ik_request_res.error_code.val != MoveItErrorCodes.SUCCESS):
raise Exception("Could not compute inverse kinematics")
return ik_request_res.solution.joint_state.position
def _make_request_msg(root_link, tip_link, target_pose, robot_object, joints):
"""
This method generates an ik request message for the kdl_ik_service. The message is
of the type moveit_msgs/PositionIKRequest and contains all information
contained in the parameter.
:param root_link: The first link of the chain of joints to be altered
:param tip_link: The last link of the chain of joints to be altered
:param target_pose: A list of two lists, the first is the pose in world coordinate frame
the second is the orientation as quanternion in world coordinate frame
:param robot_object: The robot for which the ik should be generated
:param joints: A list of joint names between the root_link and tip_link that should be altered.
:return: A moveit_msgs/PositionIKRequest message containing all the information from the parameter
"""
pose_sta = PoseStamped()
pose_sta.header.frame_id = root_link
pose_sta.header.stamp = rospy.Time.now()
tar_pose = target_pose[0]
tar_rotation = target_pose[1]
pose = Pose()
pose.position.x = tar_pose[0]
pose.position.y = tar_pose[1]
pose.position.z = tar_pose[2]
pose.orientation.x = tar_rotation[0]
pose.orientation.y = tar_rotation[1]
pose.orientation.z = tar_rotation[2]
pose.orientation.w = tar_rotation[3]
pose_sta.pose = pose
robot_state = RobotState()
joint_state = JointState()
names, poses = _get_position_for_all_joints(robot_object)
joint_state.name = joints
joint_state.position = _get_position_for_joints(robot_object, joints)
joint_state.velocity = [0.0 for x in range(len(joints))]
joint_state.effort = [0.0 for x in range(len(joints))]
#joint_state.name = names
#joint_state.position = poses
robot_state.joint_state = joint_state
msg_request = PositionIKRequest()
#msg_request.group_name = "arm"
msg_request.ik_link_name = tip_link
msg_request.pose_stamped = pose_sta
msg_request.avoid_collisions = False
msg_request.robot_state = robot_state
#msg_request.timeout = rospy.Duration(secs=10000)
#msg_request.attempts = 1000
return msg_request
def get_ik(self, phase, eef_pose=None):
self.phase = phase
if self.verbose:
print 'Computing IK for', phase
rospy.wait_for_service('/compute_ik')
try:
req = rospy.ServiceProxy('/compute_ik', GetPositionIK)
ik = PositionIKRequest()
rs = RobotState()
ik.group_name = self.planning_group_name
rs.joint_state.header.frame_id = "/" + self.reference_frame
names = []
vals = []
if phase == 'present':
badly_named_var = self.joint_states_presentation_pose
else:
badly_named_var = self.joint_states_ik_seed
for name, val in badly_named_var.items():
if name not in self.joints_to_exclude:
names.append(name)
vals.append(val)
# vals.append(val[0])
rs.joint_state.name = names
rs.joint_state.position = vals
ik.ik_link_name = self.palm_link
ik.robot_state = rs
if eef_pose is None: # just for the grasp
ik.pose_stamped = self.generate_grasp_pose(phase)
else: # for the presentation pose
ik.pose_stamped = eef_pose
ik.timeout.secs = 3.0 # [s]
ik.attempts = 2
# print '\nIK message:', ik
res = req(ik)
outcome = res.error_code.val
# print res
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def get_ik(self, target_pose, end_effector_link=None):
if self.use_moveit:
service_request = PositionIKRequest()
service_request.group_name = self.arm_planning_group
service_request.ik_link_name = end_effector_link
service_request.pose_stamped = target_pose
service_request.timeout.secs = self.planning_time
service_request.avoid_collisions = True
try:
resp = self.compute_ik(ik_request = service_request)
print(resp)
return resp
except rospy.ServiceException, e:
print ("Service call failed: %s"%e)
def compute_ik(self, joint_angles, target):
"""
Compute inverse-kinematics.
Parameters
----------
joint_angles : list of float
Joint angles of the robot in radians.
target : list of float
Target pose in cartesian coordinates and euler angles in radians.
i.e. [x, y, z, rx, ry, rz]
Returns
-------
target_angles : list of float or int.
Target joint angles if a solution is found, else -31 which
indicates a solution cannot be found.
"""
quaternion = Rotation.from_euler(
"zyx", [target[-1], target[-2], target[-3]]).as_quat()
# create request
req = PositionIKRequest()
req.timeout = rospy.Duration(0.05)
req.ik_link_name = "left_gripper/grasping_frame"
req.pose_stamped.header = Header()
req.pose_stamped.header.frame_id = "world"
req.pose_stamped.pose.position.x = target[0]
req.pose_stamped.pose.position.y = target[1]
req.pose_stamped.pose.position.z = target[2]
req.pose_stamped.pose.orientation.x = quaternion[0]
req.pose_stamped.pose.orientation.y = quaternion[1]
req.pose_stamped.pose.orientation.z = quaternion[2]
req.pose_stamped.pose.orientation.w = quaternion[3]
req.robot_state.joint_state.name = self.JOINT_NAMES
req.robot_state.joint_state.position = joint_angles
req.group_name = "left_arm"
req.avoid_collisions = False
# get ik result
res = self._ik(req)
ik_result = res.solution
if res.error_code.val == -31:
rospy.loginfo("Cannot find IK solution")
return -31
# todo: make this generic
return ik_result.joint_state.position[4:11]
def get_ik(self, target_pose, avoid_collisions=False, joint_states=None, ik_constraints=None):
"""
Computes the inverse kinematics for a given pose. It returns a JointState.
@param target_pose - A given pose of type PoseStamped.
@param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false.
@param joint_states - initial joint configuration of type JointState from which the IK solution is computed.
If set to None, the current joint state is retrieved automatically.
@param ik_constraints - Set constraints of type Constraints for computing the IK solution.
"""
service_request = PositionIKRequest()
service_request.group_name = self._name
service_request.ik_link_name = self._move_group_commander.get_end_effector_link()
service_request.pose_stamped = target_pose
service_request.timeout.secs = 1
service_request.avoid_collisions = avoid_collisions
if ik_constraints is not None:
service_request.constraints = ik_constraints
if joint_states is None:
service_request.robot_state.joint_state = self.get_joints_state()
else:
service_request.robot_state.joint_state = joint_states
try:
resp = self._compute_ik(ik_request=service_request)
# Check if error_code.val is SUCCESS=1
if resp.error_code.val != 1:
if resp.error_code.val == -10:
rospy.logerr("Unreachable point: Start state in collision")
elif resp.error_code.val == -12:
rospy.logerr("Unreachable point: Goal state in collision")
elif resp.error_code.val == -31:
rospy.logerr("Unreachable point: No IK solution")
else:
rospy.logerr("Unreachable point (error: %s)" % resp.error_code)
return
else:
return resp.solution.joint_state
except rospy.ServiceException as e:
rospy.logerr("Service call failed: %s" % e)
def get_ik(self, pose):
rospy.loginfo('Waiting for service')
rospy.wait_for_service('compute_ik')
ik_request = PositionIKRequest()
ik_request.group_name = self.group.get_name()
ik_request.robot_state = self.robot.get_current_state()
ik_request.ik_link_name = self.group.get_end_effector_link()
pose_stmp = self.start_pose
pose_stmp.pose = pose
ik_request.pose_stamped = pose_stmp
ik_request.timeout.secs = 10
ik_request.attempts = 0
try:
ik_service = rospy.ServiceProxy('compute_ik', GetPositionIK)
resp = ik_service(ik_request)
print(resp.solution.joint_state)
return resp
except rospy.ServiceException, e:
rospy.logerr('Error with IK service')
def get_ik(self, position, orientation, planner_time_limit=0.5):
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = self.BASE_LINK
gripper_pose_stamped.header.stamp = rospy.Time.now()
gripper_pose_stamped.pose = Pose(Point(*position),
Quaternion(*orientation))
service_request = PositionIKRequest()
service_request.group_name = self.ARM
service_request.ik_link_name = self.TIP_LINK
service_request.pose_stamped = gripper_pose_stamped
service_request.timeout.secs = planner_time_limit
service_request.avoid_collisions = True
try:
resp = self.compute_ik(ik_request=service_request)
return resp
except rospy.ServiceException, e:
print("Service call failed: %s" % e)
rospy.wait_for_service('compute_ik')
get_position_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
target = PoseStamped()
target.header.frame_id = 'base_link'
x, y, z = 1.5, 0.6, 1.5
target.pose.position.x = x
target.pose.position.y = y
target.pose.position.z = z
qx, qy, qz, qw = 0.0, -0.3, 0.0, 0.954
target.pose.orientation.x = qx
target.pose.orientation.y = qy
target.pose.orientation.z = qz
target.pose.orientation.w = qw
service_request = PositionIKRequest()
service_request.group_name = 'irb_5400'
#service_request.robot_state = initial_state
#service_request.ik_link_name = 'link_6'
service_request.pose_stamped = target
service_request.timeout.secs = 0.1
service_request.avoid_collisions = False
rospy.loginfo("Request = {0}".format(service_request))
resp = get_position_ik(service_request)
rospy.loginfo("Response = {0}".format(resp))
#rospy.loginfo("Base position = [{0},{1},{2}".format(resp.solution.joint_state.position[0],resp.solution.joint_state.position[1],resp.solution.joint_state.position[2]))
position = resp.solution.joint_state.position
rospy.loginfo("Arm position = [{0},{1},{2},{3},{4},{5}]".format(
def cart_trj_to_jnt_trj_tracik_solver(self, arm, trj, targ_obj):
rospy.wait_for_service('panda/compute_ik')
compute_ik = rospy.ServiceProxy('panda/compute_ik', GetPositionIK)
angle_set = []
pose_set = []
robot_state = arm.robot.get_current_state()
if (targ_obj == 1):
m2b_frame_tf = arm.tfBuffer.lookup_transform(
'panda_link0', 'blue_frame', rospy.Time(0))
elif (targ_obj == 2):
m2b_frame_tf = arm.tfBuffer.lookup_transform(
'panda_link0', 'red_frame', rospy.Time(0))
b_pose = Pose()
b_pose.position = m2b_frame_tf.transform.translation
b_pose.orientation = m2b_frame_tf.transform.rotation
p_init = arm.get_pose().pose
q_init = robot_state.joint_state.position[0:len(arm.joint_names)]
p_targ = p_init
p_targ_stamped = PoseStamped()
p_targ_stamped.header.frame_id = arm.root
p_targ_stamped.header.stamp = rospy.Time.now()
p_targ_stamped.pose = p_targ
msg_req = PositionIKRequest()
msg_req.group_name = arm.group.get_name()
msg_req.robot_state = robot_state
msg_req.pose_stamped = p_targ_stamped
msg_req.timeout.secs = 2
msg_req.avoid_collisions = False
jointAngle = compute_ik(msg_req)
q_targ = list(
jointAngle.solution.joint_state.position[0:len(arm.joint_names)])
if jointAngle.error_code.val == -31:
arm.write_to_csv(p_init, q_init, p_targ)
print 'No IK solution found at initial pose'
return None
angle_set.append(q_targ)
pose_set.append(p_targ)
q_init = list(q_targ)
p_init = p_targ
for i in range(0, len(trj)):
p_targ.position.x = trj[i, 0]
p_targ.position.y = trj[i, 1]
p_targ.position.z = trj[i, 2]
p_targ.orientation = orient_towards_object(arm.tfBuffer, p_init,
b_pose)
if (pose_dist(b_pose, p_targ) <= 0.18):
break
q_init_tmp = list(q_init)
q_init_tmp.append(0.0)
q_init_tmp.append(0.0)
ik_joint_state = JointState()
ik_joint_state.header.frame_id = robot_state.joint_state.header.frame_id
ik_joint_state.header.stamp = rospy.Time.now()
ik_joint_state.name = robot_state.joint_state.name
ik_joint_state.position = q_init_tmp
ik_robot_state = RobotState()
ik_robot_state.joint_state = ik_joint_state
p_targ_stamped = PoseStamped()
p_targ_stamped.header.frame_id = arm.root
p_targ_stamped.header.stamp = rospy.Time.now()
p_targ_stamped.pose = p_targ
msg_req = PositionIKRequest()
msg_req.group_name = arm.group.get_name()
msg_req.robot_state = ik_robot_state
msg_req.pose_stamped = p_targ_stamped
msg_req.timeout.secs = 2
msg_req.avoid_collisions = False
jointAngle = compute_ik(msg_req)
q_targ = list(jointAngle.solution.joint_state.
position[0:len(arm.joint_names)])
if jointAngle.error_code.val == -31:
arm.write_to_csv(p_init, q_init, p_targ)
print 'No IK solution found at trajectory point %d' % i
return None
angle_set.append(q_targ)
pose_set.append(p_targ)
q_init = list(q_targ)
p_init = p_targ
return angle_set, pose_set
def __init__(self, config):
rospy.init_node("gqcnn_base_grasp", anonymous=True)
# Moveit! setup
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.arm = moveit_commander.MoveGroupCommander('arm')
self.gripper = moveit_commander.MoveGroupCommander('gripper')
self.arm.set_start_state_to_current_state()
self.arm.set_pose_reference_frame('base')
self.arm.set_planner_id('SBLkConfigDefault')
self.arm.set_planning_time(10)
self.arm.set_max_velocity_scaling_factor(0.04)
self.arm.set_max_acceleration_scaling_factor(0.04)
self.arm.set_goal_orientation_tolerance(0.1)
self.arm.set_workspace([-2, -2, -2, 2, 2, 2])
self.gripper.set_goal_joint_tolerance(0.2)
rospy.loginfo(self.arm.get_pose_reference_frame()) #base
rospy.loginfo(self.arm.get_planning_frame()) #world
# messgae filter for image topic, need carefully set./camera/depth_registered/sw_registered/image_rect,,/camera/rgb/image_rect_color
rospy.loginfo('wait_for_message') ##############
rospy.wait_for_message("/camera/rgb/image_raw", Image) ###########
rospy.wait_for_message("/camera/depth/image", Image) ###########
rospy.loginfo('start_callback') ###########
self.image_sub = message_filters.Subscriber("/camera/rgb/image_raw",
Image)
self.depth_sub = message_filters.Subscriber("/camera/depth/image",
Image)
self.camera_info_topic = "/camera/rgb/camera_info"
self.camera_info = rospy.wait_for_message(self.camera_info_topic,
CameraInfo)
rospy.loginfo(self.camera_info.header.frame_id) ######
self.ts = message_filters.ApproximateTimeSynchronizer(
[self.image_sub, self.depth_sub], 1, 1)
self.ts.registerCallback(self.cb)
self.color_msg = Image()
self.depth_msg = Image()
self.mask = Image()
# bounding box for objects
self.bounding_box = BoundingBox()
self.bounding_box.maxX = 420
self.bounding_box.maxY = 250
self.bounding_box.minX = 90
self.bounding_box.minY = 40
self.bridge = CvBridge()
# transform listener
self.listener = tf.TransformListener()
# compute_ik service
self.ik_srv = rospy.ServiceProxy('/compute_ik', GetPositionIK)
rospy.loginfo("Waiting for /compute_ik service...")
self.ik_srv.wait_for_service()
rospy.loginfo("Connected!")
self.service_request = PositionIKRequest()
self.service_request.group_name = 'arm'
self.service_request.timeout = rospy.Duration(2)
self.service_request.avoid_collisions = True
# signal
self.start = 0
def _obtain_linear_buffer_path(self):
assert self._planner == 'moveit'
ik_cands = self._compute_ik_cands(
self._linear_buffer_start_pose).joint_angles
if len(ik_cands) == 0:
rospy.logerr('No IK candidates found for the requested pose')
return None
print(f'Checking {int(len(ik_cands)/6)} candidates')
# Build robot state message
robot_state = self._moveit_robot.get_current_state()
robot_state.attached_collision_objects = self._moveit_scene.get_attached_objects(
).values()
robot_state.joint_state.velocity = []
robot_state.joint_state.effort = []
robot_state.joint_state.position = [0, 0, 0, 0, 0, 0]
# Build IK request message
ik_request = PositionIKRequest()
ik_request.group_name = "manipulator"
ik_request.robot_state = robot_state
ik_request.avoid_collisions = True
ik_request.pose_stamped.header.frame_id = 'world'
ik_request.timeout = rospy.Duration(secs=self.PLAN_TIMEOUT)
for ikc_i in range(0, len(ik_cands), 6):
start_joint_state = np.radians(ik_cands[ikc_i:ikc_i + 6]).tolist()
robot_state.joint_state.position[0:6] = start_joint_state
is_valid = self._check_state_validity(robot_state, "manipulator",
Constraints()).valid
if not is_valid:
print(f'Invalid: {ik_cands[ikc_i:ikc_i+6]}')
continue
print(f'Valid: {ik_cands[ikc_i:ikc_i+6]}')
did_succeed, motion_path = self._plan_linear_motion(
start_joint_state, [
item for sublist in self._linear_buffer_waypoints
for item in sublist
],
ik_request=ik_request)
if did_succeed:
print('Found valid begin joint state!')
# Clip motion path
eu_index = len([
item for sublist in self.
_linear_buffer_waypoints[:self.
_linear_buffer_execution_size]
for item in sublist
])
motion_path = motion_path[:eu_index]
# Insert initial TCP motion
motion_path.insert(0, start_joint_state)
return motion_path
print(
'Couldn\'t find valid joint state after going through all candidates'
)
return None
position_1.pose.orientation.z = 0
position_1.pose.orientation.w = 1
position_2 = PoseStamped()
position_2.header = ref_frame
position_2.pose.position.x = 0.7
position_2.pose.position.y = 0
position_2.pose.position.z = 0.6
position_2.pose.orientation.x = 0
position_2.pose.orientation.y = 0
position_2.pose.orientation.z = 0
position_2.pose.orientation.w = 1
way_points = [position_1.pose, position_2.pose]
rospy.loginfo("Start IK Request")
ik_request = PositionIKRequest()
ik_request.group_name = "manipulator"
ik_request.avoid_collisions = True
ik_request.pose_stamped = position_1
response = ik_solution(ik_request)
rospy.loginfo("End IK Request")
# group.set_pose_reference_frame("base_link")
# group.set_pose_targets(way_points, end_effector_link="racket")
# plan = group.plan()
# path = plan.joint_trajectory
# sz = len(path.points)
# plan_array = np.zeros((sz, 3))
#
# solution = response.solution.joint_state.position
# ik_array = solution[0:3]
# for p_num in range(sz):
rospy.wait_for_service('compute_ik')
get_position_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
target = PoseStamped()
target.header.frame_id = 'base_link'
x, y, z = 1.5, 0.6, 1.5
target.pose.position.x = x
target.pose.position.y = y
target.pose.position.z = z
qx, qy, qz, qw = 0.0, -0.3, 0.0, 0.954
target.pose.orientation.x = qx
target.pose.orientation.y = qy
target.pose.orientation.z = qz
target.pose.orientation.w = qw
service_request = PositionIKRequest()
service_request.group_name = 'irb_5400'
#service_request.robot_state = initial_state
#service_request.ik_link_name = 'link_6'
service_request.pose_stamped = target
service_request.timeout.secs= 0.1
service_request.avoid_collisions = False
rospy.loginfo("Request = {0}".format(service_request))
resp = get_position_ik(service_request)
rospy.loginfo("Response = {0}".format(resp))
#rospy.loginfo("Base position = [{0},{1},{2}".format(resp.solution.joint_state.position[0],resp.solution.joint_state.position[1],resp.solution.joint_state.position[2]))
position = resp.solution.joint_state.position
rospy.loginfo("Arm position = [{0},{1},{2},{3},{4},{5}]".format(position[0], position[1], position[2], position[3], position[4], position[5]))
def plan_grasps(group, grasps):
cur_state = RobotState(
joint_state=JointState(
name=group.get_joints(),
position=group.get_current_joint_values()
)
)
for i, grasp in enumerate(grasps):
rospy.loginfo("%s/%s" % (i+1, len(grasps)))
approach_plan = None
try:
ik_solution = _compute_ik(
ik_request = PositionIKRequest(
group_name = "arm_left_torso",
robot_state = cur_state,
pose_stamped = PoseStamped(
header=Header(frame_id="base_link"),
pose=grasp.approach
),
avoid_collisions = True
)
)
except:#, err:
#print "compute_ik failed, "+str(err)
print traceback.format_exc()
rospy.logerr("Failed to call IK")
continue
if ik_solution.error_code.val != MoveItErrorCodes.SUCCESS:
rospy.logwarn("Failed to find valid IK solution "+str(ik_solution.error_code.val))
continue
# Plan Approach
response, success = get_cartesian_path(
group_name=group.get_name(),
start_state=ik_solution.solution,
waypoints=[grasp.pregrasp],
max_step=0.01,
jump_threshold=0.0,
avoid_collisions=True
)
if not success:
continue
print "Fraction: ", response.fraction
rospy.loginfo("Cartesian path had status code: "+str(response.error_code))
if response.fraction < 1:
continue
if not check_collision(response.solution.joint_trajectory):
continue
# Plan Retreat
poses = [grasp.pregrasp]
poses.append(deepcopy(poses[-1]))
poses[-1].position.z += 0.032
poses.append(deepcopy(poses[-1]))
poses[-1].position.x = 0.4
retreat_response, success = get_cartesian_path(
group_name=group.get_name(),
start_state=RobotState(
joint_state=JointState(
name=response.solution.joint_trajectory.joint_names,
position=response.solution.joint_trajectory.points[-1].positions
)
),
waypoints=poses,
max_step=0.01,
jump_threshold=0.0,
avoid_collisions=True
)
if not success:
continue
print "Fraction: ", retreat_response.fraction
rospy.loginfo("Cartesian path had status code: "+str(retreat_response.error_code))
if retreat_response.fraction < 1:
continue
if not check_collision(retreat_response.solution.joint_trajectory):
continue
group.set_planner_id("RRTConnectkConfigDefault")
group.set_workspace([-3, -3, -3, 3, 3, 3])
for t in [1,3]:
group.set_planning_time(t)
plan = group.plan(ik_solution.solution.joint_state)
if len(plan.joint_trajectory.points) > 0:
approach_plan = plan
break
if not approach_plan:
rospy.logwarn("Failed to find plan to approach pose")
continue
if not check_collision(approach_plan.joint_trajectory):
continue
rospy.loginfo("Found grasp")
rospy.loginfo(ik_solution.solution.joint_state)
yield grasp, approach_plan, response.solution, retreat_response.solution
def main():
## Initialization ##
rospy.init_node("push_button")
trajectory_generator = TrajectoryGenerator()
trajectory_executor = TrajectoryExecutor()
button_finder = ButtonLocationSubscriber()
arm_joints = ["shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint",
"forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"]
arm_action = FollowTrajectoryClient('arm_controller', arm_joints)
#scene_pub = rospy.Publisher('planning_scene',
# PlanningScene,
# queue_size=10)
rospy.loginfo("Waiting for get_position_ik...")
rospy.wait_for_service('compute_ik')
get_position_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
rospy.loginfo("...connected.")
## Map button world frame pose to base_link frame pose of gripper ##
button_center = None
while button_center is None:
rospy.sleep(1.0)
button_center = button_finder.getButtonLocation()
#button_center = [1.08, 0.06, 1.02]
desired_gripper_translation = 0.08
push_depth = -.015
gripper_offset = .16645
desired_gripper_translation += gripper_offset
push_depth -= gripper_offset
pre_push_pose = PoseGoal([button_center[0]-desired_gripper_translation,
button_center[1], button_center[2], 0,0,0])
print pre_push_pose.pose
pre_push_posture = GripperPostureGoal(-0.5)
push = [button_center[0] + push_depth, button_center[1], button_center[2]]
post_push_retreat = [button_center[0]-desired_gripper_translation,
button_center[1], button_center[2]]
post_push_posture = GripperPostureGoal(1.0)
post_push_pose = PoseGoal([post_push_retreat[0], post_push_retreat[1], post_push_retreat[2],
0, 0, 0])
tuck_state = JointSpaceGoal([1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0])
pre_push_goals = [pre_push_pose, pre_push_posture]
pre_push_trajectories = trajectory_generator.generate_trajectories(pre_push_goals, get_approval=True)
pre_push_state = trajectory_generator.get_virtual_arm_state()
print pre_push_state.joint_state.position
trajectory_executor.execute_trajectories(pre_push_trajectories, error_checking=False)
ps = PoseStamped()
ps.header.frame_id = "base_link"
ps.pose.position.x = push[0]
ps.pose.position.y = push[1]
ps.pose.position.z = push[2]
ps.pose.orientation.w = 1.0
ik_request = PositionIKRequest()
ik_request.group_name = 'arm'
ik_request.pose_stamped = ps
while True:
response = get_position_ik(ik_request)
print response
user = raw_input("Accept position? Y/N")
if user == 'Y':
break
push_state = response.solution.joint_state.position[6:13]
#state = get_position_ik(group = 'arm')
print response.solution.joint_state.name[6:13]
print push_state
print pre_push_state
raw_input('Press ENTER to proceed')
arm_action.move_to(push_state)
ps = PoseStamped()
ps.header.frame_id = "base_link"
ps.pose.position.x = push[0] - .08
ps.pose.position.y = push[1]
ps.pose.position.z = push[2]
ps.pose.orientation.w = 1.0
ik_request = PositionIKRequest()
ik_request.group_name = 'arm'
ik_request.pose_stamped = ps
while True:
response = get_position_ik(ik_request)
print response
user = raw_input("Accept position? Y/N")
if user == 'Y':
break
inter_state_1 = response.solution.joint_state.position[6:13]
print inter_state_1
"""
ps = PoseStamped()
ps.header.frame_id = "base_link"
ps.pose.position.x = push[0] - .025 * 2.0/3.0
ps.pose.position.y = push[1]
ps.pose.position.z = push[2]
ps.pose.orientation.w = 1.0
ik_request = PositionIKRequest()
ik_request.group_name = 'arm'
ik_request.pose_stamped = ps
while True:
response = get_position_ik(ik_request)
print response
user = raw_input("Accept position? Y/N")
if user == 'Y':
break
inter_state_2 = response.solution.joint_state.position[6:13]
print inter_state_2
"""
"""
ps = PlanningScene()
ps.is_diff = True
print ps
ls_l = LinkScale()
ls_l.link_name = "l_gripper_finger_link"
print "+++++SCALE: ", ls_l.scale
ls_l.scale = .01
ls_r = LinkScale()
ls_r.link_name = "r_gripper_finger_link"
ls_r.scale = .01
ps.link_scale.append(ls_l)
ps.link_scale.append(ls_r)
scene_pub.publish(ps)
"""
#arm_action.move_to(inter_state_1)
#arm_action.move_to(inter_state_2)
post_push_state = pre_push_state.joint_state.position
#arm_action.move_to(post_push_state)
post_push_goals = [tuck_state]
"""
ps = PlanningScene()
ps.is_diff = True
ls_l = LinkScale()
ls_l.link_name = "l_gripper_finger_link"
ls_l.scale = .06
ls_r = LinkScale()
ls_r.link_name = "r_gripper_finger_link"
ls_r.scale = .06
ps.link_scale.append(ls_l)
ps.link_scale.append(ls_r)
scene_pub.publish(ps)
"""
trajectory_generator.clear_virtual_arm_state()
post_push_trajectories = trajectory_generator.generate_trajectories(post_push_goals, get_approval=True)
trajectory_executor.execute_trajectories(post_push_trajectories, error_checking = False)
def ik(pose, collision=False, init_robot_state=None, num_attempts=50):
global ik_database
# verify if the pose is in collision
#robot = moveit_commander.RobotCommander()
#group_arm_name = "robot_arm"
#group = moveit_commander.MoveGroupCommander(group_arm_name)
SAMPLE_STATE = 1
CURRENT_STATE = 0
pos_dif_threshold = .6
ori_dif_threshold = 360
if init_robot_state is None:
# initialize the robot state by linearly interpolate the pose, and compute IK for each position
# use the last one as the init
init_robot_state = robot.get_current_state()
num_waypoints = 20
init_pose_stamped = group.get_current_pose()
init_pose = init_pose_stamped.pose
last_valid_state, robot_state, error_code_val = linear_ik(
pose,
collision,
init_pose,
init_robot_state,
num_waypoints,
num_attempts=20)
if error_code_val:
# IK is solved using initial value
return last_valid_state, robot_state, error_code_val
# if we want collision check too, then check if we can get self-collision-free but in-collision-with-env state
# if we found this case, then it means we don't need to do sample anymore
if collision:
last_valid_state, robot_state, error_code_val = linear_ik(
pose,
False,
init_pose,
init_robot_state,
num_waypoints,
num_attempts=20)
#rospy.logwarn('turnning on collision in IK for init robot state. error code: %d' % (error_code_val))
if error_code_val and not gripper_collison_free(robot_state):
# if gripper in collision, we will fail sample too
return last_valid_state, robot_state, False
# otherwise, we can try to get self-collison-free by sample
# otherwise, current_state may not be a good init value
if SAMPLE_STATE:
if ik_database is None:
# create database
print('creating IK database...')
joint_names = group.get_active_joints()
robot_state = robot.get_current_state()
# print('joint_names: ')
# print(joint_names)
# print('robot_State joint_state:')
# print(robot_state.joint_state.name)
robot_states = []
robot_poses = []
database_idx = 0
while True:
if database_idx == database_sz:
break
print('database_idx: %d' % (database_idx))
joint_values_i = group.get_random_joint_values()
# print('joint value: ')
# print(joint_values_i)
pos = list(robot_state.joint_state.position)
for k in range(6): #6DOF
pos[k] = joint_values_i[k]
robot_state.joint_state.position = pos
# check collision for the state
if not self_collision_free(robot_state):
# in collision
continue
pose = fk(robot_state)
# add constraints on the height
if pose.position.z <= 0.2:
continue
# print('robot_state value:')
# print(robot_state.joint_state.position)
# fk to get pose
database_idx += 1
robot_poses.append(pose)
robot_states.append(copy.deepcopy(robot_state))
ik_database = list(zip(robot_poses, robot_states))
# store this file to DISK
f = open(ik_database_path, 'w')
pickle.dump(ik_database, f)
f.close()
print('successfully stored IK database.')
# sample several states, and compute pose. find nearest several points as initialization
# do linear_ik from those initialization to see if solution exists
last_valid_state = None
for i in range(database_sz):
init_pose = ik_database[i][0]
init_robot_state = ik_database[i][1]
num_waypoints = 5
pos_dif, ori_dif = pose_distance(pose, init_pose)
#print('pos_dif: %f, ori_dif: %f' % (pos_dif, ori_dif))
if pos_dif <= pos_dif_threshold and ori_dif <= ori_dif_threshold * np.pi / 180:
# use this as an initalization and try linear_ik
last_valid_state, robot_state, error_code_val = \
linear_ik(pose, collision, init_pose, init_robot_state, num_waypoints, num_attempts=5)
if error_code_val:
# valid IK found, return it
#print('sample state IK success.')
return last_valid_state, robot_state, error_code_val
print('sample_state IK failed too.')
return last_valid_state, robot_state, error_code_val
# call MoveIt! IK service to compute the robot state from the pose
rospy.wait_for_service('compute_ik')
# generate message
ik_request = PositionIKRequest()
ik_request.group_name = "robot_arm"
ik_request.robot_state = init_robot_state
#ik_request.avoid_collisions = False
ik_request.avoid_collisions = collision
pose_stamped = group.get_current_pose()
pose_stamped.pose = pose
ik_request.pose_stamped = pose_stamped
ik_request.timeout = rospy.Duration.from_sec(0.005) # from kinematics.yaml
#ik_request.attempts = num_attempts
try:
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
resp = compute_ik(ik_request)
except rospy.ServiceException as e:
#print("Service call failed: %s"%e)
sys.exit(1)
robot_state = resp.solution
error_code = resp.error_code
if error_code.val != 1:
#print('IK with collision [%d] failed.' % (collision))
return init_robot_state, robot_state, (error_code.val == 1)
if collision == True:
init_robot_state = robot_state
#print('collision IK success')
return init_robot_state, robot_state, True
if self_collision_free(robot_state):
#print('self_collision_free. IK success.')
init_robot_state = robot_state
return init_robot_state, robot_state, True
#print('self_collision. IK fail.')
# otherwise, fail
return init_robot_state, robot_state, False
def lookat(pose, torso_ik):
'''
query arm_ik server for joint_position which put arm_7_link to pose
@param arm_ik: arm_ik service proxy
@param t: translation
@param q: rotation as quaternion
@param link: frame in which pose (t, q) is defined
@param seed: initial joint positions for ik calculation, in None current joint pos of arm is used.
@return: tuple of joint_positions or None if ik was not found
'''
#print "lookat"
# get joint names for arm from parameter server
joint_names = None
try:
joint_names = rospy.get_param(
"torso_controller/joint_names") # real hardware
except KeyError:
pass
if joint_names is None:
print "Could not get arm joint_names from parameter server."
return None
msg = rospy.wait_for_message("/torso_controller/state",
JointTrajectoryControllerState)
t = pose[0]
q = pose[1]
# create and send ik request
req = PositionIKRequest()
req.group_name = "lookat"
req.timeout = rospy.Duration(1.0)
req.attempts = 3
req.ik_link_name = "lookat_focus_frame"
req.robot_state.joint_state.position = msg.actual.positions
req.robot_state.joint_state.name = msg.joint_names
req.pose_stamped.header.frame_id = 'base_link'
req.pose_stamped.pose.position.x = t[0]
req.pose_stamped.pose.position.y = t[1]
req.pose_stamped.pose.position.z = t[2]
req.pose_stamped.pose.orientation.x = q[0]
req.pose_stamped.pose.orientation.y = q[1]
req.pose_stamped.pose.orientation.z = q[2]
req.pose_stamped.pose.orientation.w = q[3]
# try to get inverse kinecmatics for at least 3 times
resp = torso_ik(req)
#print resp
# report sucess or return None on error
#if resp.error_code.val == resp.error_code.SUCCESS:
#result = list(resp.solution.joint_state.position)
#return result
result = []
if resp.error_code.val == resp.error_code.SUCCESS:
for name, position in zip(list(resp.solution.joint_state.name),
list(resp.solution.joint_state.position)):
if name in msg.joint_names:
result.append(position)
return result
joint_names = rospy.get_param(
"arm_controller/joints") # simulation
except KeyError:
pass
if joint_names is None:
print "Could not get arm joint_names from parameter server."
return None
msg = rospy.wait_for_message(
"/arm_controller/state", JointTrajectoryControllerState)
if seed is None:
seed = msg.actual.positions
# create and send ik request
req = PositionIKRequest()
req.group_name = "arm"
req.timeout = rospy.Duration(1.0)
req.ik_link_name = link
#req.ik_link_names = msg.joint_names
req.robot_state.joint_state.position = seed
req.robot_state.joint_state.name = msg.joint_names
req.pose_stamped.header.frame_id = 'base_link'
req.pose_stamped.pose.position.x = t[0]
req.pose_stamped.pose.position.y = t[1]
req.pose_stamped.pose.position.z = t[2]
req.pose_stamped.pose.orientation.x = q[0]
req.pose_stamped.pose.orientation.y = q[1]
req.pose_stamped.pose.orientation.z = q[2]
req.pose_stamped.pose.orientation.w = q[3]
req.attempts = 3
