vs_controller.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 #  Copyright (c) 2012, Georgia Institute of Technology
00005 #  All rights reserved.
00006 #
00007 #  Redistribution and use in source and binary forms, with or without
00008 #  modification, are permitted provided that the following conditions
00009 #  are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #     notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #     copyright notice, this list of conditions and the following
00015 #     disclaimer in the documentation and/or other materials provided
00016 #     with the distribution.
00017 #  * Neither the name of the Georgia Institute of Technology nor the names of
00018 #     its contributors may be used to endorse or promote products derived
00019 #     from this software without specific prior written permission.
00020 #
00021 #  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 #  'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 #  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 #  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 #  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 #  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 #  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 #  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 #  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 #  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 #  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 #  POSSIBILITY OF SUCH DAMAGE.
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 # import tabletop_pushing.position_feedback_push_node as pn
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         # Setup parameters
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         # Initialize vel controller
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         # self.init_arm_servo()
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         # self.pn.gripper_pose()
00082 
00083     # util
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 # PoseStamped
00110       #pose.pose.orientation.x = -0.7071
00111       #pose.pose.orientation.y = 0
00112       #pose.pose.orientation.z = 0.7071
00113       #pose.pose.orientation.w = 0
00114 
00115       self.pn.move_to_cart_pose(pose, 'l')
00116       rospy.loginfo(pose)
00117       return {'result': 0}
00118       # return VisualServoTwistResponse('{0}')
00119 
00120     def handle_twist_request(self, req):
00121       t = req.twist # service call
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       # self.vel_sat = self.vel_sat_param
00128       try:
00129         twist = TwistStamped()
00130         twist.header.stamp = rospy.Time(0)
00131         twist.header.frame_id = 'torso_lift_link'
00132 
00133         # task specific gain control. we know that we are aligned in y... 
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         # after(before) adjustment
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


visual_servo
Author(s): Stephan Lee
autogenerated on Wed Nov 27 2013 11:44:03