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.0002934322105607734, -0.38304632633953606, 6.931483707006691e-07, 0.8526320037121202, 5.69952198326007e-07, -0.47039657856235184, -0.00029228225570943067]])
00014
00015 if __name__ == '__main__':
00016 rospy.init_node('test_move_circ_interface')
00017 init_pos()
00018
00019
00020 pose = sci.gen_pose(pos=[-0.2, 0.0, 0.8], rpy=[0, 0.0, 0.0])
00021 profile = Profile()
00022 profile.vel = 0.2
00023 profile.accl = 0.1
00024 profile.profile_type = Profile.SINOID
00025
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
00034 pose = sci.gen_pose(pos=[0.0, 0.0, 0.8], rpy=[0.0, 0.0, 0.0])
00035 start_angle = 180.0 * math.pi / 180.0
00036 end_angle = 0.0 * math.pi / 180.0
00037 profile = Profile()
00038 profile.vel = 0.4
00039 profile.accl = 0.3
00040 profile.profile_type = Profile.SINOID
00041
00042
00043 success, message = sci.move_circ(pose, "world", start_angle, end_angle, 0.2, profile)
00044 if success:
00045 rospy.loginfo(message)
00046 else:
00047 rospy.logerr(message)
00048
00049
00050 pose = sci.gen_pose(pos=[0.2, 0.0, 0.8], rpy=[0.0, math.pi/2, 0.0])
00051 profile = Profile()
00052 profile.vel = 0.3
00053 profile.accl = 0.3
00054 profile.profile_type = Profile.SINOID
00055
00056
00057 success, message = sci.move_lin(pose, "world", profile)
00058 if success:
00059 rospy.loginfo(message)
00060 else:
00061 rospy.logerr(message)
00062
00063
00064 pose = sci.gen_pose(pos=[0.0, -0.2, 0.8], rpy=[0.0, 0.0, math.pi/2])
00065 profile = Profile()
00066 profile.vel = 0.3
00067 profile.accl = 0.3
00068 profile.profile_type = Profile.SINOID
00069
00070
00071 success, message = sci.move_lin(pose, "world", profile)
00072 if success:
00073 rospy.loginfo(message)
00074 else:
00075 rospy.logerr(message)
00076
00077
00078 pose = sci.gen_pose(pos=[0.0, 0.0, 0.8], rpy=[0.0, 0.0, math.pi/2])
00079 start_angle = 180.0 * math.pi / 180.0
00080 end_angle = 0.0 * math.pi / 180.0
00081 profile = Profile()
00082 profile.vel = 0.4
00083 profile.accl = 0.3
00084 profile.profile_type = Profile.SINOID
00085
00086
00087 success, message = sci.move_circ(pose, "world", start_angle, end_angle, 0.2, profile)
00088 if success:
00089 rospy.loginfo(message)
00090 else:
00091 rospy.logerr(message)
00092
00093
00094 pose = sci.gen_pose(pos=[0.0, 0.2, 0.8], rpy=[0.0, math.pi/2, 0.0])
00095 profile = Profile()
00096 profile.vel = 0.3
00097 profile.accl = 0.3
00098 profile.profile_type = Profile.SINOID
00099
00100
00101 success, message = sci.move_lin(pose, "world", profile)
00102 if success:
00103 rospy.loginfo(message)
00104 else:
00105 rospy.logerr(message)
00106
00107
00108 pose = sci.gen_pose(pos=[-0.2, 0.0, 0.8], rpy=[0.0, 0.0, 0.0])
00109 profile = Profile()
00110 profile.vel = 0.3
00111 profile.accl = 0.3
00112 profile.profile_type = Profile.SINOID
00113
00114
00115 success, message = sci.move_lin(pose, "world", profile)
00116 if success:
00117 rospy.loginfo(message)
00118 else:
00119 rospy.logerr(message)
00120
00121 init_pos()