Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 import roslib; roslib.load_manifest('visual_servo')
00035 import rospy
00036 from geometry_msgs.msg import TwistStamped
00037 from geometry_msgs.msg import PoseStamped
00038 import tf
00039 from visual_servo.srv import *
00040 from std_srvs.srv import Empty
00041
00042
00043 import visual_servo.position_feedback_push_node as pn
00044 import sys
00045 import numpy as np
00046 from math import sqrt
00047
00048 LEFT_ARM_READY_JOINTS = np.matrix([[0.62427649, 0.4556137,
00049 1.63411927, -2.11931035,
00050 -15.38839978, -1.64163257,
00051 -17.2947453]]).T
00052 RIGHT_ARM_READY_JOINTS = np.matrix([[-0.42427649, 0.0656137,
00053 -1.43411927, -2.11931035,
00054 15.78839978, -1.64163257,
00055 8.64421842e+01]]).T
00056
00057 class VNode:
00058
00059 def __init__(self):
00060
00061
00062 self.vel_sat_param = rospy.get_param('~vel_sat_param', 0.20)
00063 self.vel_scale_param = rospy.get_param('~vel_scale_param', 0.20)
00064
00065
00066 self.pn = pn.PositionFeedbackPushNode()
00067 self.l_cart_twist_pub = self.pn.l_arm_cart_vel_pub
00068 self.l_cart_pub = self.pn.l_arm_cart_pub
00069 self.pn.init_spine_pose()
00070 self.pn.init_head_pose(self.pn.head_pose_cam_frame)
00071 self.pn.init_arms()
00072 self.pn.gripper_open()
00073
00074
00075 self.pn.switch_to_cart_controllers()
00076
00077 self.which_arm = 'l'
00078
00079 rospy.loginfo('Done moving to robot initial pose')
00080
00081
00082
00083
00084
00085 def init_arm_servo(self, which_arm='l'):
00086 '''
00087 Move the arm to the initial pose to be out of the way for viewing the
00088 tabletop
00089 '''
00090 if which_arm == 'l':
00091 ready_joints = LEFT_ARM_READY_JOINTS
00092 else:
00093 ready_joints = RIGHT_ARM_READY_JOINTS
00094
00095 rospy.loginfo('Moving %s_arm to init servo pose' % which_arm)
00096 self.pn.set_arm_joint_pose(ready_joints, which_arm)
00097 rospy.loginfo('Moved %s_arm to init servo pose' % which_arm)
00098
00099
00100 def adjust_velocity(self, vel):
00101 ret = vel * self.vel_scale
00102 if ret > self.vel_sat:
00103 ret = self.vel_sat
00104 elif -ret > self.vel_sat:
00105 ret = -self.vel_sat
00106 return ret
00107
00108 def handle_pose_request(self, req):
00109 pose = req.p
00110
00111
00112
00113
00114
00115 self.pn.move_to_cart_pose(pose, 'l')
00116 rospy.loginfo(pose)
00117 return {'result': 0}
00118
00119
00120 def handle_twist_request(self, req):
00121 t = req.twist
00122 e = req.error
00123 self.vel_scale = sqrt(e + self.vel_scale_param)
00124 self.vel_sat = sqrt(e * self.vel_sat_param)
00125 if self.vel_sat > 0.08:
00126 self.vel_sat = 0.08
00127
00128 try:
00129 twist = TwistStamped()
00130 twist.header.stamp = rospy.Time(0)
00131 twist.header.frame_id = 'torso_lift_link'
00132
00133
00134 twist.twist.linear.x = self.adjust_velocity(t.twist.linear.x)
00135 twist.twist.linear.y = self.adjust_velocity(t.twist.linear.y)
00136 twist.twist.linear.z = self.adjust_velocity(t.twist.linear.z)
00137 twist.twist.angular.x = self.adjust_velocity(t.twist.angular.x)
00138 twist.twist.angular.y = self.adjust_velocity(t.twist.angular.y)
00139 twist.twist.angular.z = self.adjust_velocity(t.twist.angular.z)
00140 if self.which_arm == 'l':
00141 vel_pub = self.pn.l_arm_cart_vel_pub
00142 posture_pub = self.pn.l_arm_vel_posture_pub
00143 else:
00144 vel_pub = self.pn.r_arm_cart_vel_pub
00145 posture_pub = self.pn.r_arm_vel_posture_pub
00146
00147 m = self.pn.get_desired_posture('l')
00148 posture_pub.publish(m)
00149 vel_pub.publish(twist)
00150
00151
00152 rospy.loginfo('[e=%.4f][sca=%.4f][sat=%.4f] x:%+.3f(%+.3f) y:%+.3f(%+.3f) z:%+.3f(%+.3f)', e, self.vel_scale, self.vel_sat, \
00153 twist.twist.linear.x, t.twist.linear.x, \
00154 twist.twist.linear.y, t.twist.linear.y, \
00155 twist.twist.linear.z, t.twist.linear.z)
00156 rospy.loginfo('[angular] x:%+.3f(%+.3f) y:%+.3f(%+.3f) z:%+.3f(%+.3f)', \
00157 twist.twist.angular.x, t.twist.angular.x, \
00158 twist.twist.angular.y, t.twist.angular.y, \
00159 twist.twist.angular.z, t.twist.angular.z)
00160 except rospy.ServiceException, e:
00161 self.pn.stop_moving_vel('l')
00162
00163 return VisualServoTwistResponse()
00164
00165 def handle_init_request(self, req):
00166 self.pn.init_arms()
00167 self.pn.gripper_open()
00168 return EmptyResponse()
00169
00170 if __name__ == '__main__':
00171 try:
00172 node = VNode()
00173
00174 rospy.loginfo('Done initializing... Now advertise the Service')
00175 rospy.Service('vs_pose', VisualServoPose , node.handle_pose_request)
00176 rospy.Service('vs_twist', VisualServoTwist , node.handle_twist_request)
00177 rospy.Service('vs_init', Empty , node.handle_init_request)
00178 rospy.loginfo('Ready to move arm')
00179 rospy.spin()
00180
00181 except rospy.ROSInterruptException: pass