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 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):
#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 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 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()
