def move(self, title, pos=[0.0, 0.0, 0.0], rpy=None, hold_duration=None): rospy.loginfo(title) if rpy is not None: self._rpy = rpy pose = sci.gen_pose(pos, self._rpy) success = sci.move_lin(pose, self._world, self._profile) if hold_duration is not None: time.sleep(hold_duration) return success
#! /usr/bin/env python import math import rospy from geometry_msgs.msg import Pose from cob_cartesian_controller.msg import Profile import simple_cartesian_interface as sci if __name__ == '__main__': rospy.init_node('test_move_circ_interface') pose = sci.gen_pose(pos=[0.0, 0.7, 1.0], rpy=[0.0, 0.0, 0.0]) start_angle = 0.0 * math.pi / 180.0 end_angle = 90.0 * math.pi / 180.0 profile = Profile() profile.vel = 0.2 profile.accl = 0.1 #profile.profile_type = Profile.SINOID profile.profile_type = Profile.RAMP success, message = sci.move_circ(pose, "world", start_angle, end_angle, 0.3, profile) if success: rospy.loginfo(message) else: rospy.logerr(message)
from geometry_msgs.msg import Pose from cob_cartesian_controller.msg import Profile from simple_script_server.simple_script_server import simple_script_server import simple_cartesian_interface as sci def init_pos(): sss = simple_script_server() sss.move("arm", [[-0.00032004093963244884, -0.7064191894021441, -1.577532922958369e-06, 1.4183686971944311, 1.2084352562169443e-05, -0.6913530502577565, -0.0002663056533762642]]) if __name__ == '__main__': rospy.init_node('test_move_lin_interface') init_pos() pose = sci.gen_pose(pos=[0.0, 0.0, 0.9], rpy=[0.0, 0.0, 0.0]) profile = Profile() profile.vel = 0.1 profile.accl = 0.05 #profile.profile_type = Profile.SINOID profile.profile_type = Profile.RAMP success, message = sci.move_lin(pose, "world", profile) if success: rospy.loginfo(message) else: rospy.logerr(message) rospy.sleep(3.0)
#! /usr/bin/env python import math import rospy from geometry_msgs.msg import Pose from cob_cartesian_controller.msg import Profile import simple_cartesian_interface as sci if __name__ == '__main__': rospy.init_node('test_move_lin_interface') pose = sci.gen_pose(pos=[-0.9, 0.0, 0.0], rpy=[0.0, 0.0, 0.0]) profile = Profile() profile.vel = 0.2 profile.accl = 0.1 #profile.profile_type = Profile.SINOID profile.profile_type = Profile.RAMP success, message = sci.move_lin(pose, "world", profile) if success: rospy.loginfo(message) else: rospy.logerr(message)
# See the License for the specific language governing permissions and # limitations under the License. from math import radians import rospy from geometry_msgs.msg import Pose from cob_cartesian_controller.msg import Profile import simple_cartesian_interface as sci if __name__ == '__main__': rospy.init_node('test_move_santa') # Step 1 pose = sci.gen_pose(pos=[-1.0, 0.0, 0.0], rpy=[radians(45.0), 0.0, radians(-25.0)]) profile = Profile() profile.vel = 0.1 profile.accl = 0.2 profile.profile_type = Profile.SINOID success, message = sci.move_lin(pose, "world", profile) rospy.sleep(4.0) # Step 2 pose = sci.gen_pose(pos=[-0.5, 0.5, 0.0], rpy=[radians(-45.0), 0.0, radians(-30.0)]) profile = Profile() profile.vel = 0.1
def init_pos(): sss = simple_script_server() sss.move("arm", [[ -0.0002934322105607734, -0.38304632633953606, 6.931483707006691e-07, 0.8526320037121202, 5.69952198326007e-07, -0.47039657856235184, -0.00029228225570943067 ]]) if __name__ == '__main__': rospy.init_node('test_move_circ_interface') init_pos() pose = sci.gen_pose(pos=[-0.2, 0.0, 0.8], rpy=[0, 0.0, 0.0]) profile = Profile() profile.vel = 0.2 profile.accl = 0.1 profile.profile_type = Profile.SINOID #profile.profile_type = Profile.RAMP success, message = sci.move_lin(pose, "world", profile) if success: rospy.loginfo(message) else: rospy.logerr(message) pose = sci.gen_pose(pos=[0.0, 0.0, 0.8], rpy=[0.0, 0.0, 0.0]) start_angle = 180.0 * math.pi / 180.0 end_angle = 0.0 * math.pi / 180.0
# # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import math import rospy from geometry_msgs.msg import Pose from cob_cartesian_controller.msg import Profile import simple_cartesian_interface as sci if __name__ == '__main__': rospy.init_node('test_move_lin_interface') pose = sci.gen_pose(pos=[-0.9, 0.0, 0.0], rpy=[0.0, 0.0, 0.0]) profile = Profile() profile.vel = 0.2 profile.accl = 0.1 #profile.profile_type = Profile.SINOID profile.profile_type = Profile.RAMP success, message = sci.move_lin(pose, "world", profile) if success: rospy.loginfo(message) else: rospy.logerr(message)