def get_fk(self, angles, frame='torso_lift_link', link=-1): # get FK pose of a list of joint angles for the arm
fk_request = GetPositionFKRequest()
fk_request.header.frame_id = frame
fk_request.header.stamp = rospy.Time.now()
fk_request.fk_link_names = self.fk_info.kinematic_solver_info.link_names
fk_request.robot_state.joint_state.header = fk_request.header
fk_request.robot_state.joint_state.position = angles #self.joint_state_act.positions
fk_request.robot_state.joint_state.name = self.fk_info.kinematic_solver_info.joint_names
print 'fk_request:', fk_request
try:
return self.fk_pose_proxy(fk_request).pose_stamped[link]
except rospy.ServiceException, e:
rospy.loginfo("FK service did not process request: %s" %str(e))
def get_fk(self, msg, frame='/torso_lift_link'): # get FK pose of a list of joint angles for the arm
fk_request = GetPositionFKRequest()
fk_request.header.frame_id = frame
#fk_request.header.stamp = rospy.Time.now()
fk_request.fk_link_names = self.fk_info.kinematic_solver_info.link_names
fk_request.robot_state.joint_state.position = self.joint_state_act.positions
fk_request.robot_state.joint_state.name = self.fk_info.kinematic_solver_info.joint_names
#print "FK Request: %s " %fk_request
try:
fk_response = self.fk_pose_proxy(fk_request)
return fk_response.pose_stamped[-1]
except rospy.ServiceException, e:
rospy.loginfo("FK service did not process request: %s" %str(e))
def _setup_fk(self):
'''Sets up services for forward kinematics.'''
side = self._side()
# Get FK info service.
fk_info_srv_name = PR2_SERVICE_PREFIX + side + SERVICE_FK_INFO_POSTFIX
rospy.wait_for_service(fk_info_srv_name)
fk_info_srv = rospy.ServiceProxy(fk_info_srv_name,
GetKinematicSolverInfo)
ks_info = fk_info_srv().kinematic_solver_info
rospy.loginfo('FK info service has responded for ' + side + ' arm.')
# Get FK service.
fk_srv_name = PR2_SERVICE_PREFIX + side + SERVICE_FK_POSTFIX
rospy.wait_for_service(fk_srv_name)
self.fk_srv = rospy.ServiceProxy(fk_srv_name,
GetPositionFK,
persistent=True)
rospy.loginfo('FK service has responded for ' + side + ' arm.')
# Set up common parts of an FK request.
self.fk_request = GetPositionFKRequest()
self.fk_request.header.frame_id = BASE_LINK
self.fk_request.fk_link_names = ks_info.link_names
self.fk_request.robot_state.joint_state.name = ks_info.joint_names
def __init__(self):
rospy.init_node("test_robot_get_fk")
self.joint_state = JointState()
self.rate = 1
r = rospy.Rate(self.rate)
q = quaternion_from_euler(0, 0, 0)
#rospy.loginfo(q)
rospy.wait_for_service('test_arm_kinematics/get_fk')
rospy.wait_for_service('test_arm_kinematics/get_fk_solver_info')
get_fk_proxy = rospy.ServiceProxy('test_arm_kinematics/get_fk',
GetPositionFK,
persistent=True)
get_fk_solver_info_proxy = rospy.ServiceProxy(
'test_arm_kinematics/get_fk_solver_info', GetKinematicSolverInfo)
left_arm_solver_info = get_fk_solver_info_proxy()
rospy.Subscriber('joint_states', JointState, self.getJointState)
self.request = GetPositionFKRequest()
self.request.robot_state.joint_state = JointState()
self.request.robot_state.joint_state.header.frame_id = 'base_actuator'
self.request.robot_state.joint_state.name = left_arm_solver_info.kinematic_solver_info.joint_names
self.request.robot_state.joint_state.position = [0] * len(
self.request.robot_state.joint_state.name)
self.request.header.frame_id = "base_actuator"
self.request.fk_link_names = list()
self.request.fk_link_names.append("base_actuator")
self.request.fk_link_names.append("link1")
self.request.fk_link_names.append("link2")
self.request.fk_link_names.append("link3")
self.request.fk_link_names.append("link4")
self.request.fk_link_names.append("link5")
self.request.fk_link_names.append("link6")
self.request.fk_link_names.append("link7")
self.request.fk_link_names.append("link8")
self.request.fk_link_names.append("link9")
while not rospy.is_shutdown():
try:
response = get_fk_proxy(self.request)
hand_link = response.pose_stamped[0]
q = list()
q.append(hand_link.pose.orientation.x)
q.append(hand_link.pose.orientation.y)
q.append(hand_link.pose.orientation.z)
q.append(hand_link.pose.orientation.w)
rpy = euler_from_quaternion(q)
rospy.loginfo(rpy)
rospy.loginfo(response.pose_stamped)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
r.sleep()
def FK(self, arm, q):
fk_req = GetPositionFKRequest()
fk_req.header.frame_id = 'torso_lift_link'
if arm == 0:
fk_req.fk_link_names.append('r_wrist_roll_link')
else:
fk_req.fk_link_names.append('l_wrist_roll_link')
fk_req.robot_state.joint_state.name = self.joint_names_list[arm]
fk_req.robot_state.joint_state.position = q
fk_resp = self.fk_srv[arm].call(fk_req)
if fk_resp.error_code.val == fk_resp.error_code.SUCCESS:
x = fk_resp.pose_stamped[0].pose.position.x
y = fk_resp.pose_stamped[0].pose.position.y
z = fk_resp.pose_stamped[0].pose.position.z
pos = [x, y, z]
q1 = fk_resp.pose_stamped[0].pose.orientation.x
q2 = fk_resp.pose_stamped[0].pose.orientation.y
q3 = fk_resp.pose_stamped[0].pose.orientation.z
q4 = fk_resp.pose_stamped[0].pose.orientation.w
quat = [q1,q2,q3,q4]
rot = tr.quaternion_to_matrix(quat)
else:
rospy.logerr('Forward kinematics failed')
return None, None
return pos, rot
def __init__(self, side_prefix, gui):
self.side_prefix = side_prefix
self.gui = gui
# Set up Inversse Kinematics services
fk_info_srv_name = ('pr2_' + self._side() + '_arm_kinematics/get_fk_solver_info')
fk_srv_name = 'pr2_' + self._side() + '_arm_kinematics/get_fk'
rospy.loginfo('Waiting for FK info service to respond.')
rospy.wait_for_service(fk_info_srv_name)
fk_info_srv = rospy.ServiceProxy(fk_info_srv_name, GetKinematicSolverInfo)
solver_info = fk_info_srv()
self.fk_joints = solver_info.kinematic_solver_info.joint_names
self.fk_limits = solver_info.kinematic_solver_info.limits
print(solver_info)
rospy.loginfo('Waiting for FK service to respond.')
rospy.wait_for_service(fk_srv_name)
self.fk_srv = rospy.ServiceProxy(fk_srv_name,
GetPositionFK,
persistent=True)
# Set up common parts of an FK request
self.fk_request = GetPositionFKRequest()
# self.fk_request.timeout = rospy.Duration(4.0)
self.fk_request.fk_link_names = solver_info.kinematic_solver_info.link_names
def get_fk(self,
angles,
frame='torso_lift_link',
link=-1): # get FK pose of a list of joint angles for the arm
fk_request = GetPositionFKRequest()
fk_request.header.frame_id = frame
fk_request.header.stamp = rospy.Time.now()
fk_request.fk_link_names = self.fk_info.kinematic_solver_info.link_names
fk_request.robot_state.joint_state.header = fk_request.header
fk_request.robot_state.joint_state.position = angles #self.joint_state_act.positions
fk_request.robot_state.joint_state.name = self.fk_info.kinematic_solver_info.joint_names
print 'fk_request:', fk_request
try:
return self.fk_pose_proxy(fk_request).pose_stamped[link]
except rospy.ServiceException, e:
rospy.loginfo("FK service did not process request: %s" % str(e))
def FK(self, arm, q):
if rospy.is_shutdown():
sys.exit()
if arm != 1:
arm = 0
fk_req = GetPositionFKRequest()
fk_req.header.frame_id = 'torso_lift_link'
if arm == 0:
fk_req.fk_link_names.append('r_wrist_roll_link')
else:
fk_req.fk_link_names.append('l_wrist_roll_link')
fk_req.robot_state.joint_state.name = self.joint_names_list[arm]
fk_req.robot_state.joint_state.position = q
fk_resp = GetPositionFKResponse()
fk_resp = self.fk_srv[arm].call(fk_req)
if fk_resp.error_code.val == fk_resp.error_code.SUCCESS:
x = fk_resp.pose_stamped[0].pose.position.x
y = fk_resp.pose_stamped[0].pose.position.y
z = fk_resp.pose_stamped[0].pose.position.z
q1 = fk_resp.pose_stamped[0].pose.orientation.x
q2 = fk_resp.pose_stamped[0].pose.orientation.y
q3 = fk_resp.pose_stamped[0].pose.orientation.z
q4 = fk_resp.pose_stamped[0].pose.orientation.w
quat = [q1, q2, q3, q4]
# Transform point from wrist roll link to actuator
ret1 = self.transform_in_frame([x, y, z], quat, self.off_point)
ret2 = np.matrix(quat).T
else:
rospy.logerr('Forward kinematics failed')
ret1, ret2 = None, None
return ret1, ret2
def get_fk(self, msg):
#print "get_fk of %s" %str(msg)
if (self.joint_state):
fk_request = GetPositionFKRequest()
fk_request.header.frame_id = '/torso_lift_link'
fk_request.fk_link_names = self.fk_info.kinematic_solver_info.link_names
fk_request.robot_state.joint_state.position = self.joint_state
fk_request.robot_state.joint_state.name = self.fk_info.kinematic_solver_info.joint_names
else:
rospy.loginfo("No Joint States Available Yet")
try:
self.curr_pose = self.fk_pose_proxy(fk_request)
self.pose_out.publish(self.curr_pose.pose_stamped[-1])
# print "Pose from FK: %s" %str(self.curr_pose.pose_stamped[-1])
except rospy.ServiceException, e:
rospy.loginfo("FK service did not process request: %s" % str(e))
def get_fk(self,
msg,
frame='/torso_lift_link'
): # get FK pose of a list of joint angles for the arm
fk_request = GetPositionFKRequest()
fk_request.header.frame_id = frame
#fk_request.header.stamp = rospy.Time.now()
fk_request.fk_link_names = self.fk_info.kinematic_solver_info.link_names
fk_request.robot_state.joint_state.position = self.joint_state_act.positions
fk_request.robot_state.joint_state.name = self.fk_info.kinematic_solver_info.joint_names
#print "FK Request: %s " %fk_request
try:
fk_response = self.fk_pose_proxy(fk_request)
return fk_response.pose_stamped[-1]
except rospy.ServiceException, e:
rospy.loginfo("FK service did not process request: %s" % str(e))
def get_fk(self, msg):
#print "get_fk of %s" %str(msg)
if (self.joint_state):
fk_request = GetPositionFKRequest()
fk_request.header.frame_id = '/torso_lift_link'
fk_request.fk_link_names = self.fk_info.kinematic_solver_info.link_names
fk_request.robot_state.joint_state.position = self.joint_state
fk_request.robot_state.joint_state.name = self.fk_info.kinematic_solver_info.joint_names
else:
rospy.loginfo("No Joint States Available Yet")
try:
self.curr_pose = self.fk_pose_proxy(fk_request)
self.pose_out.publish(self.curr_pose.pose_stamped[-1])
# print "Pose from FK: %s" %str(self.curr_pose.pose_stamped[-1])
except rospy.ServiceException, e:
rospy.loginfo("FK service did not process request: %s" %str(e))
def get_fk(self, fk_links, robot_state=None, frame_id=None):
'''
Solves forward kinematics.
In general, it is better to use the planning scene or the state transformer get_transform function between
robot links rather than this function.
**Args:**
**fk_links ([string]):** Links for which you want FK solutions
*robot_state (arm_navigation_msgs.mgs.RobotState):* The state of the robot during forward kinematics.
If no state is passed in the state in the planning scene will be used.
*frame_id (string):* The frame ID in which you want the returned poses. Note that the FK service may use
TF so we recommend only using the robot frame. If no frame is specified, the robot frame is used.
**Returns:**
A list of geometry_msgs.msg.PoseStamped corresponding to forward kinematic solutions for fk_links.
**Raises:**
**rospy.ServiceException:** if there is a problem with the service call
**Warning:**
Calls a service which may use TF! Recommended that you only ask for poses in the robot frame.
'''
req = GetPositionFKRequest()
req.header.frame_id = frame_id
if not frame_id:
req.header.frame_id = self._psi.robot_frame
req.header.stamp = rospy.Time(0)
req.robot_state = robot_state
if not robot_state:
req.robot_state = self._psi.get_robot_state()
req.fk_link_names = fk_links
res = self._fk_service(req)
if res.error_code.val != res.error_code.SUCCESS or not res.pose_stamped:
raise ArmNavError('Forward kinematics failed',
error_code=res.error_code)
return res.pose_stamped
def get_fk(self, fk_links, robot_state=None, frame_id=None):
'''
Solves forward kinematics.
In general, it is better to use the planning scene or the state transformer get_transform function between
robot links rather than this function.
**Args:**
**fk_links ([string]):** Links for which you want FK solutions
*robot_state (arm_navigation_msgs.mgs.RobotState):* The state of the robot during forward kinematics.
If no state is passed in the state in the planning scene will be used.
*frame_id (string):* The frame ID in which you want the returned poses. Note that the FK service may use
TF so we recommend only using the robot frame. If no frame is specified, the robot frame is used.
**Returns:**
A list of geometry_msgs.msg.PoseStamped corresponding to forward kinematic solutions for fk_links.
**Raises:**
**rospy.ServiceException:** if there is a problem with the service call
**Warning:**
Calls a service which may use TF! Recommended that you only ask for poses in the robot frame.
'''
req = GetPositionFKRequest()
req.header.frame_id = frame_id
if not frame_id:
req.header.frame_id = self._psi.robot_frame
req.header.stamp = rospy.Time(0)
req.robot_state = robot_state
if not robot_state:
req.robot_state = self._psi.get_robot_state()
req.fk_link_names = fk_links
res = self._fk_service(req)
if res.error_code.val != res.error_code.SUCCESS or not res.pose_stamped:
raise ArmNavError('Forward kinematics failed', error_code=res.error_code)
return res.pose_stamped
def __init__(self):
rospy.init_node("fanuc_robot_get_fk")
self.rate = 1
r = rospy.Rate(self.rate)
self.joints=[]
self.links=[]
rospy.wait_for_service('get_fk')
rospy.wait_for_service('get_fk_solver_info')
get_fk_proxy = rospy.ServiceProxy('get_fk', GetPositionFK, persistent=True)
get_fk_solver_info_proxy = rospy.ServiceProxy('get_fk_solver_info', GetKinematicSolverInfo)
solver_info = get_fk_solver_info_proxy()
rospy.loginfo(solver_info)
for joint in solver_info.kinematic_solver_info.joint_names:
self.joints.append(joint)
rospy.loginfo("Adding joint " + str(joint))
for link in solver_info.kinematic_solver_info.link_names:
self.links.append(link)
rospy.loginfo("Adding link " + str(link))
self.request.robot_state.joint_state = JointState()
self.request.robot_state.joint_state.header.frame_id = 'torso_link'
self.request.robot_state.joint_state.name = self.joints
self.request.robot_state.joint_state.position = [0] * len(self.joints)
self.request.robot_state.joint_state.position[0] = 1.0
self.request = GetPositionFKRequest()
self.request.fk_link_names = self.links
self.request.header.frame_id = "torso_link"
while not rospy.is_shutdown():
try:
response = get_fk_proxy(self.request)
rospy.loginfo(response)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
r.sleep()
def FK(self, arm, q):
rospy.logwarn('Currently ignoring the arm parameter.')
fk_req = GetPositionFKRequest()
fk_req.header.frame_id = 'torso_lift_link'
fk_req.fk_link_names.append('r_wrist_roll_link')
fk_req.robot_state.joint_state.name = self.joint_names_list
fk_req.robot_state.joint_state.position = q
fk_resp = GetPositionFKResponse()
fk_resp = self.fk_srv.call(fk_req)
if fk_resp.error_code.val == fk_resp.error_code.SUCCESS:
x = fk_resp.pose_stamped[0].pose.position.x
y = fk_resp.pose_stamped[0].pose.position.y
z = fk_resp.pose_stamped[0].pose.position.z
ret = np.matrix([x, y, z]).T
else:
rospy.logerr('Forward kinematics failed')
ret = None
return ret
def FK(self, q):
fk_req = GetPositionFKRequest()
fk_req.header.frame_id = 'torso_lift_link'
fk_req.fk_link_names.append(self.arm + '_wrist_roll_link')
fk_req.robot_state.joint_state.name = self.joint_names_list
fk_req.robot_state.joint_state.position = q
fk_resp = self.fk_srv.call(fk_req)
if fk_resp.error_code.val == fk_resp.error_code.SUCCESS:
x = fk_resp.pose_stamped[0].pose.position.x
y = fk_resp.pose_stamped[0].pose.position.y
z = fk_resp.pose_stamped[0].pose.position.z
pos = np.mat([x, y, z]).T
q1 = fk_resp.pose_stamped[0].pose.orientation.x
q2 = fk_resp.pose_stamped[0].pose.orientation.y
q3 = fk_resp.pose_stamped[0].pose.orientation.z
q4 = fk_resp.pose_stamped[0].pose.orientation.w
quat = [q1, q2, q3, q4]
mat = np.mat(tf_trans.quaternion_matrix(quat))
mat[:3, 3] = pos
return mat
rospy.logerr('Forward kinematics failed')
return None
def push_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo("%s: preempted" % ACTION_NAME)
self.action_server.set_preempted()
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
current_state = rospy.wait_for_message("l_arm_controller/state", FollowJointTrajectoryFeedback)
start_angles = current_state.actual.positions
full_state = rospy.wait_for_message("/joint_states", JointState)
req = GetPositionFKRequest()
req.header.frame_id = "base_link"
req.fk_link_names = [GRIPPER_LINK_FRAME]
req.robot_state.joint_state = full_state
if not self.set_planning_scene_diff_client():
rospy.logerr("%s: failed to set planning scene diff" % ACTION_NAME)
self.action_server.set_aborted()
return
pose = self.get_fk_srv(req).pose_stamped[0].pose
frame_id = "base_link"
approachpos = [pose.position.x, pose.position.y, pose.position.z]
approachquat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
push_distance = 0.40
grasppos = [pose.position.x, pose.position.y - push_distance, pose.position.z]
graspquat = [0.00000, 0.00000, 0.70711, -0.70711]
# attach object to gripper, they will move together
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
# disable collisions between attached object and table
collision_operation1 = CollisionOperation()
collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
collision_operation1.object2 = collision_support_surface_name
collision_operation1.operation = CollisionOperation.DISABLE
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
collision_operation2.penetration_distance = 0.02
ordered_collision_operations = OrderedCollisionOperations()
ordered_collision_operations.collision_operations = [collision_operation1, collision_operation2]
res = self.check_cartesian_path_lists(
approachpos,
approachquat,
grasppos,
graspquat,
start_angles,
frame=frame_id,
ordered_collision_operations=ordered_collision_operations,
)
error_code_dict = {
ArmNavigationErrorCodes.SUCCESS: 0,
ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1,
ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2,
ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3,
ArmNavigationErrorCodes.PLANNING_FAILED: 4,
}
trajectory_len = len(res.trajectory.joint_trajectory.points)
trajectory = [res.trajectory.joint_trajectory.points[i].positions for i in range(trajectory_len)]
vels = [res.trajectory.joint_trajectory.points[i].velocities for i in range(trajectory_len)]
times = [res.trajectory.joint_trajectory.points[i].time_from_start for i in range(trajectory_len)]
error_codes = [error_code_dict[error_code.val] for error_code in res.trajectory_error_codes]
# if even one code is not 0, abort
if sum(error_codes) != 0:
for ind in range(len(trajectory)):
rospy.loginfo("error code " + str(error_codes[ind]) + " pos : " + self.pplist(trajectory[ind]))
rospy.loginfo("")
for ind in range(len(trajectory)):
rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() + "vels: " + self.pplist(vels[ind]))
rospy.logerr("%s: attempted push failed" % ACTION_NAME)
self.action_server.set_aborted()
return
req = FilterJointTrajectoryRequest()
req.trajectory = res.trajectory.joint_trajectory
req.trajectory.points = req.trajectory.points[1:] # skip zero velocity point
req.allowed_time = rospy.Duration(2.0)
filt_res = self.trajectory_filter_srv(req)
goal = FollowJointTrajectoryGoal()
goal.trajectory = filt_res.trajectory
goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id)
rospy.sleep(0.5)
self.trajectory_controller.send_goal(goal)
self.trajectory_controller.wait_for_result()
state = self.trajectory_controller.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.sleep(0.5)
sound_msg = self.stop_audio_recording_srv(True)
self.action_server.set_succeeded(PushObjectResult(sound_msg.recorded_sound))
return
rospy.logerr("%s: attempted push failed" % ACTION_NAME)
self.stop_audio_recording_srv(False)
self.action_server.set_aborted()
def __init__(self):
rospy.init_node("pr2lite_get_fk")
self.joint_state = JointState()
self.rate = 5
r = rospy.Rate(self.rate)
q = quaternion_from_euler(0, 0, 0)
hand_pub = rospy.Publisher('hand_pose', PoseStamped)
side = "right"
service_prefix = "pr2lite_"
base_frame = "_arm_shelf_link"
rospy.wait_for_service(service_prefix + side +
'_arm_kinematics/get_fk')
rospy.wait_for_service(service_prefix + side +
'_arm_kinematics/get_fk_solver_info')
get_fk_proxy = rospy.ServiceProxy(service_prefix + side +
'_arm_kinematics/get_fk',
GetPositionFK,
persistent=True)
get_fk_solver_info_proxy = rospy.ServiceProxy(
service_prefix + side + '_arm_kinematics/get_fk_solver_info',
GetKinematicSolverInfo)
left_arm_solver_info = get_fk_solver_info_proxy()
self.left_arm_joints = list()
for joint in left_arm_solver_info.kinematic_solver_info.joint_names:
self.left_arm_joints.append(joint)
rospy.loginfo('Adding joint ' + str(joint))
self.request = GetPositionFKRequest()
self.request.robot_state.joint_state = JointState()
self.request.robot_state.joint_state.header.frame_id = side + base_frame
self.request.robot_state.joint_state.name = left_arm_solver_info.kinematic_solver_info.joint_names
self.request.robot_state.joint_state.position = [0] * len(
self.request.robot_state.joint_state.name)
#self.request.robot_state.joint_state.position[0] = 1.0
for joint in self.request.robot_state.joint_state.name:
self.request.robot_state.joint_state.position[
self.request.robot_state.joint_state.name.index(joint)] = 0
rospy.Subscriber('joint_states', JointState, self.getJointState)
self.request.header.frame_id = side + base_frame
self.request.fk_link_names = list()
# self.request.fk_link_names.append("left_shoulder_pan_link")
# self.request.fk_link_names.append("left_shoulder_lift_link")
# self.request.fk_link_names.append("left_arm_roll_link")
# self.request.fk_link_names.append("left_elbow_link")
# self.request.fk_link_names.append("left_forearm_link")
# self.request.fk_link_names.append("left_wrist_link")
self.request.fk_link_names.append("right_wrist_roll_link")
while not rospy.is_shutdown():
self.request.header.stamp = rospy.Time.now()
self.request.robot_state.joint_state.header.stamp = rospy.Time.now(
)
#rospy.loginfo(self.request)
try:
response = get_fk_proxy(self.request)
hand_link = response.pose_stamped[0]
# q = list()
# q.append(hand_link.pose.orientation.x)
# q.append(hand_link.pose.orientation.y)
# q.append(hand_link.pose.orientation.z)
# q.append(hand_link.pose.orientation.w)
#rpy = euler_from_quaternion(q)
#rospy.loginfo(rpy)
msg = PoseStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = side + base_frame
msg.pose = hand_link.pose
rospy.loginfo(response.pose_stamped)
hand_pub.publish(msg)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
r.sleep()
def push_object(self, goal):
if self.action_server.is_preempt_requested():
rospy.loginfo('%s: preempted' % ACTION_NAME)
self.action_server.set_preempted()
collision_object_name = goal.collision_object_name
collision_support_surface_name = goal.collision_support_surface_name
current_state = rospy.wait_for_message('l_arm_controller/state',
FollowJointTrajectoryFeedback)
start_angles = current_state.actual.positions
full_state = rospy.wait_for_message('/joint_states', JointState)
req = GetPositionFKRequest()
req.header.frame_id = 'base_link'
req.fk_link_names = [GRIPPER_LINK_FRAME]
req.robot_state.joint_state = full_state
if not self.set_planning_scene_diff_client():
rospy.logerr('%s: failed to set planning scene diff' % ACTION_NAME)
self.action_server.set_aborted()
return
pose = self.get_fk_srv(req).pose_stamped[0].pose
frame_id = 'base_link'
approachpos = [pose.position.x, pose.position.y, pose.position.z]
approachquat = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
push_distance = 0.40
grasppos = [
pose.position.x, pose.position.y - push_distance, pose.position.z
]
graspquat = [0.00000, 0.00000, 0.70711, -0.70711]
# attach object to gripper, they will move together
obj = AttachedCollisionObject()
obj.object.header.stamp = rospy.Time.now()
obj.object.header.frame_id = GRIPPER_LINK_FRAME
obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT
obj.object.id = collision_object_name
obj.link_name = GRIPPER_LINK_FRAME
obj.touch_links = GRIPPER_LINKS
self.attached_object_pub.publish(obj)
# disable collisions between attached object and table
collision_operation1 = CollisionOperation()
collision_operation1.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
collision_operation1.object2 = collision_support_surface_name
collision_operation1.operation = CollisionOperation.DISABLE
collision_operation2 = CollisionOperation()
collision_operation2.object1 = collision_support_surface_name
collision_operation2.object2 = GRIPPER_GROUP_NAME
collision_operation2.operation = CollisionOperation.DISABLE
collision_operation2.penetration_distance = 0.02
ordered_collision_operations = OrderedCollisionOperations()
ordered_collision_operations.collision_operations = [
collision_operation1, collision_operation2
]
res = self.check_cartesian_path_lists(
approachpos,
approachquat,
grasppos,
graspquat,
start_angles,
frame=frame_id,
ordered_collision_operations=ordered_collision_operations)
error_code_dict = {
ArmNavigationErrorCodes.SUCCESS: 0,
ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED: 1,
ArmNavigationErrorCodes.PATH_CONSTRAINTS_VIOLATED: 2,
ArmNavigationErrorCodes.JOINT_LIMITS_VIOLATED: 3,
ArmNavigationErrorCodes.PLANNING_FAILED: 4
}
trajectory_len = len(res.trajectory.joint_trajectory.points)
trajectory = [
res.trajectory.joint_trajectory.points[i].positions
for i in range(trajectory_len)
]
vels = [
res.trajectory.joint_trajectory.points[i].velocities
for i in range(trajectory_len)
]
times = [
res.trajectory.joint_trajectory.points[i].time_from_start
for i in range(trajectory_len)
]
error_codes = [
error_code_dict[error_code.val]
for error_code in res.trajectory_error_codes
]
# if even one code is not 0, abort
if sum(error_codes) != 0:
for ind in range(len(trajectory)):
rospy.loginfo("error code " + str(error_codes[ind]) +
" pos : " + self.pplist(trajectory[ind]))
rospy.loginfo("")
for ind in range(len(trajectory)):
rospy.loginfo("time: " + "%5.3f " % times[ind].to_sec() +
"vels: " + self.pplist(vels[ind]))
rospy.logerr('%s: attempted push failed' % ACTION_NAME)
self.action_server.set_aborted()
return
req = FilterJointTrajectoryRequest()
req.trajectory = res.trajectory.joint_trajectory
req.trajectory.points = req.trajectory.points[
1:] # skip zero velocity point
req.allowed_time = rospy.Duration(2.0)
filt_res = self.trajectory_filter_srv(req)
goal = FollowJointTrajectoryGoal()
goal.trajectory = filt_res.trajectory
goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.1)
self.start_audio_recording_srv(InfomaxAction.PUSH, goal.category_id)
rospy.sleep(0.5)
self.trajectory_controller.send_goal(goal)
self.trajectory_controller.wait_for_result()
state = self.trajectory_controller.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.sleep(0.5)
sound_msg = self.stop_audio_recording_srv(True)
self.action_server.set_succeeded(
PushObjectResult(sound_msg.recorded_sound))
return
rospy.logerr('%s: attempted push failed' % ACTION_NAME)
self.stop_audio_recording_srv(False)
self.action_server.set_aborted()
