cartesian_controller.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002         
00003 import sys
00004 
00005 import roslib
00006 roslib.load_manifest('hrl_ellipsoidal_control')
00007 from hrl_ellipsoidal_control.ellipsoid_controller import EllipsoidController
00008 
00009 class CartesianNormalizedController(EllipsoidController):
00010     # TODO overwrite execute_cart_move
00011 
00012     def _get_ell_equiv_dist(self, ell_change_ep, ell_abs_sel, orient_quat, velocity):
00013         ell_f, rot_mat_f = self._parse_ell_move(ell_change_ep, ell_abs_sel, orient_quat)
00014         traj = self._create_ell_trajectory(ell_f, rot_mat_f, orient_quat, velocity)
00015         dist = np.linalg.norm(traj[0][0] - traj[-1][0])
00016         return dist
00017 
00018 
00019 def main():
00020     rospy.init_node("cartesian_controller", sys.argv)
00021     cart_arm = create_pr2_arm('l', PR2ArmJTransposeTask, 
00022                               controller_name='%s_cart_jt_task', 
00023                               end_link="%s_gripper_shaver45_frame", timeout=0)
00024 
00025     rospy.sleep(1)
00026     cart_controller = CartesianController(cart_arm)
00027     rospy.spin()
00028     
00029 
00030 if __name__ == "__main__":
00031     main()


hrl_ellipsoidal_control
Author(s): Kelsey Hawkins
autogenerated on Wed Nov 27 2013 11:41:49