cob_cartesian_controller: test_move

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00004 #
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #
00009 #   http://www.apache.org/licenses/LICENSE-2.0
00010 #
00011 # Unless required by applicable law or agreed to in writing, software
00012 # distributed under the License is distributed on an "AS IS" BASIS,
00013 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014 # See the License for the specific language governing permissions and
00015 # limitations under the License.
00016 
00017 
00018 from math import radians
00019 import rospy
00020 
00021 from geometry_msgs.msg import Pose
00022 from cob_cartesian_controller.msg import Profile
00023 import simple_cartesian_interface as sci
00024 
00025 if __name__ == '__main__':
00026     rospy.init_node('test_move_santa')
00027 
00028     # Step 1
00029     pose = sci.gen_pose(pos=[-1.0, 0.0, 0.0], rpy=[radians(45.0), 0.0, radians(-25.0)])
00030     profile = Profile()
00031     profile.vel = 0.1
00032     profile.accl = 0.2
00033     profile.profile_type = Profile.SINOID
00034     success, message = sci.move_lin(pose, "world", profile)
00035 
00036     rospy.sleep(4.0)
00037 
00038     # Step 2
00039     pose = sci.gen_pose(pos=[-0.5, 0.5, 0.0], rpy=[radians(-45.0), 0.0, radians(-30.0)])
00040     profile = Profile()
00041     profile.vel = 0.1
00042     profile.accl = 0.2
00043     profile.profile_type = Profile.SINOID
00044     success, message = sci.move_lin(pose, "world", profile)
00045 
00046     rospy.sleep(4.0)
00047 
00048     # Step 3
00049     pose = sci.gen_pose(pos=[0.5, 0.5, 0.0])
00050     profile = Profile()
00051     profile.vel = 0.1
00052     profile.accl = 0.2
00053     profile.profile_type = Profile.SINOID
00054     success, message = sci.move_lin(pose, "world", profile)
00055 
00056     rospy.sleep(2.0)
00057 
00058     # Step 4
00059     pose = sci.gen_pose(pos=[1.0, 0.0, 0.0])
00060     profile = Profile()
00061     profile.vel = 0.1
00062     profile.accl = 0.2
00063     profile.profile_type = Profile.SINOID
00064     success, message = sci.move_lin(pose, "world", profile)
00065 
00066     rospy.sleep(2.0)
00067 
00068     # Step 5
00069     pose = sci.gen_pose(pos=[0.0, -1.0, 0.0])
00070     profile = Profile()
00071     profile.vel = 0.1
00072     profile.accl = 0.2
00073     profile.profile_type = Profile.SINOID
00074     success, message = sci.move_lin(pose, "world", profile)
00075 
00076     rospy.sleep(2.0)
00077 
00078     # Step 6
00079     pose = sci.gen_pose(pos=[-1.0, 1.0, 0.0])
00080     profile = Profile()
00081     profile.vel = 0.1
00082     profile.accl = 0.2
00083     profile.profile_type = Profile.SINOID
00084     success, message = sci.move_lin(pose, "world", profile)
00085 
00086     rospy.sleep(2.0)
00087 
00088     # Step 7
00089     pose = sci.gen_pose(pos=[0.0, -1.0, 0.0])
00090     profile = Profile()
00091     profile.vel = 0.1
00092     profile.accl = 0.2
00093     profile.profile_type = Profile.SINOID
00094     success, message = sci.move_lin(pose, "world", profile)
00095 
00096     rospy.sleep(2.0)
00097 
00098     # Step 8
00099     pose = sci.gen_pose(pos=[1.0, 1.0, 0.0])
00100     profile = Profile()
00101     profile.vel = 0.1
00102     profile.accl = 0.2
00103     profile.profile_type = Profile.SINOID
00104     success, message = sci.move_lin(pose, "world", profile)