00001
00002 import math
00003 import rospy
00004
00005 from geometry_msgs.msg import Pose
00006 from cob_cartesian_controller.msg import Profile
00007 from simple_script_server.simple_script_server import simple_script_server
00008 import simple_cartesian_interface as sci
00009
00010
00011 def init_pos():
00012 sss = simple_script_server()
00013 sss.move("arm", [[-0.00032004093963244884, -0.7064191894021441, -1.577532922958369e-06, 1.4183686971944311, 1.2084352562169443e-05, -0.6913530502577565, -0.0002663056533762642]])
00014
00015 if __name__ == '__main__':
00016 rospy.init_node('test_move_lin_interface')
00017 init_pos()
00018
00019
00020 pose = sci.gen_pose(pos=[0.0, 0.0, 0.9], rpy=[0.0, 0.0, 0.0])
00021 profile = Profile()
00022 profile.vel = 0.1
00023 profile.accl = 0.05
00024
00025 profile.profile_type = Profile.RAMP
00026
00027 success, message = sci.move_lin(pose, "world", profile)
00028 if success:
00029 rospy.loginfo(message)
00030 else:
00031 rospy.logerr(message)
00032
00033 rospy.sleep(3.0)
00034
00035
00036 pose = sci.gen_pose(pos=[0.3, 0.0, 0.9], rpy=[0.0, 0.0, 0.0])
00037 profile = Profile()
00038 profile.vel = 0.1
00039 profile.accl = 0.05
00040
00041 profile.profile_type = Profile.RAMP
00042
00043 success, message = sci.move_lin(pose, "world", profile)
00044 if success:
00045 rospy.loginfo(message)
00046 else:
00047 rospy.logerr(message)
00048
00049 rospy.sleep(3.0)
00050
00051
00052 pose = sci.gen_pose(pos=[0.3, 0.0, 0.8], rpy=[0.0, 0.0, 0.0])
00053 profile = Profile()
00054 profile.vel = 0.1
00055 profile.accl = 0.05
00056
00057 profile.profile_type = Profile.RAMP
00058
00059 success, message = sci.move_lin(pose, "world", profile)
00060 if success:
00061 rospy.loginfo(message)
00062 else:
00063 rospy.logerr(message)
00064
00065 rospy.sleep(3.0)
00066
00067
00068 pose = sci.gen_pose(pos=[-0.3, 0.0, 0.8], rpy=[0.0, 0.0, 0.0])
00069 profile = Profile()
00070 profile.vel = 0.1
00071 profile.accl = 0.05
00072
00073 profile.profile_type = Profile.RAMP
00074
00075 success, message = sci.move_lin(pose, "world", profile)
00076 if success:
00077 rospy.loginfo(message)
00078 else:
00079 rospy.logerr(message)
00080
00081 rospy.sleep(3.0)
00082
00083
00084 pose = sci.gen_pose(pos=[-0.3, 0.0, 0.9], rpy=[0.0, 0.0, 0.0])
00085 profile = Profile()
00086 profile.vel = 0.1
00087 profile.accl = 0.05
00088
00089 profile.profile_type = Profile.RAMP
00090
00091 success, message = sci.move_lin(pose, "world", profile)
00092 if success:
00093 rospy.loginfo(message)
00094 else:
00095 rospy.logerr(message)
00096
00097 rospy.sleep(3.0)
00098
00099
00100 pose = sci.gen_pose(pos=[0.0, 0.0, 0.9], rpy=[0.0, 0.0, 0.0])
00101 profile = Profile()
00102 profile.vel = 0.1
00103 profile.accl = 0.05
00104
00105 profile.profile_type = Profile.RAMP
00106
00107 success, message = sci.move_lin(pose, "world", profile)
00108 if success:
00109 rospy.loginfo(message)
00110 else:
00111 rospy.logerr(message)
00112
00113 rospy.sleep(3.0)