def __init__(self): self.kinematics = kinematics.Kinematics('arm') self.move_arm = move_arm.MoveArm('arm') # distance from object center to pre-grasp self.pre_grasp_distance = rospy.get_param('~pre_grasp_distance') # distance from object to grasp self.grasp_distance = rospy.get_param('~grasp_distance') # height of the post-grasp over the object center self.post_grasp_height = rospy.get_param('~post_grasp_height') # at which angle of approach to start searching for a solution self.orbit_start_angle = 30.0 * math.pi / 180.0 rospy.loginfo('Pre-grasp distance: %f', self.pre_grasp_distance) rospy.loginfo('Grasp distance: %f', self.grasp_distance) rospy.loginfo('Post-grasp height: %f', self.post_grasp_height) self.sss = simple_script_server.simple_script_server() # action clients #rospy.loginfo('Waiting for 'grasp_posture_control' action') #self.hand_action = actionlib.SimpleActionClient('grasp_posture_control', object_manipulation_msgs.msg.GraspHandPostureExecutionAction) #self.hand_action.wait_for_server() #rospy.loginfo('Found action 'grasp_posture_control'') # service servers self.pickup_server = rospy.Service('pickup', mdr_behavior_msgs.srv.Pickup, self.pickup) rospy.loginfo('"pickup" service advertised')
def __init__(self, default_pitch=(math.pi / 2.0)): rospy.loginfo("Initializing arm control.") rospy.loginfo("Waiting for [%s] server..." % (self.CART_SERVER)) self.move_arm_cart_server = SimpleActionClient(self.CART_SERVER, MoveArmAction) self.move_arm_cart_server.wait_for_server() self.script_server = simple_script_server() self.pitch = default_pitch
def main(): rospy.init_node(NODE) print "==> %s started " % NODE # init print "--> initializing sss" sss = simple_script_server() print "--> setup care-o-bot for capture" # get position from parameter server position_path = rospy.get_param('position_path', None) if position_path is None: print "[ERROR]: no path for positions set" return with open(position_path, 'r') as f: positions = yaml.load(f) system_path = rospy.get_param('system_path', None) if system_path is None: print "[ERROR]: no path for system configuration set" return with open(system_path, 'r') as f: system = yaml.load(f) print "==> capturing samples" start = rospy.Time.now() t = DhHardwareTest() t.capture_loop(positions, system, sss) print "finished after %s seconds" % (rospy.Time.now() - start).to_sec()
def __init__(self): ''' Configures the calibration node Reads configuration from parameter server or uses default values ''' rospy.init_node(NODE) print "==> started " + NODE # get parameter from parameter server or set defaults self.pattern_size = rospy.get_param('~pattern_size', "9x6") self.square_size = rospy.get_param('~square_size', 0.03) self.alpha = rospy.get_param('~alpha', 0.0) self.verbose = rospy.get_param('~verbose', True) self.save_result = rospy.get_param('~save_result', False) # split pattern_size string into tuple, e.g '9x6' -> tuple(9,6) self.pattern_size = tuple((int(self.pattern_size.split("x")[0]), int(self.pattern_size.split("x")[1]))) self.camera_info = CameraInfo() self.camera_info_received = False self.latest_image = Image() self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard(self.pattern_size, self.square_size) self.detector = CheckerboardDetector(self.board) self.sss = simple_script_server() self.ts = TorsoState()
def __init__(self): ''' Configures the calibration node Reads configuration from parameter server or uses default values ''' rospy.init_node(NODE) print "==> started " + NODE # get parameter from parameter server or set defaults self.pattern_size = rospy.get_param('~pattern_size', "9x6") self.square_size = rospy.get_param('~square_size', 0.03) self.alpha = rospy.get_param('~alpha', 0.0) self.verbose = rospy.get_param('~verbose', True) self.save_result = rospy.get_param('~save_result', False) # split pattern_size string into tuple, e.g '9x6' -> tuple(9,6) self.pattern_size = tuple((int(self.pattern_size.split( "x")[0]), int(self.pattern_size.split("x")[1]))) self.camera_info = CameraInfo() self.camera_info_received = False self.latest_image = Image() self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard(self.pattern_size, self.square_size) self.detector = CheckerboardDetector(self.board) self.sss = simple_script_server() self.ts = TorsoState()
def init(): global confirm sss = simple_script_server() say("I am ready. I will initialize myself.") say("Please confirm.") confirm = False while confirm==False: rospy.sleep(0.1) # initialize components handle_head = sss.init("head") handle_torso = sss.init("torso") if handle_torso.get_error_code() != 0: return say("failed to initialize torso") handle_tray = sss.init("tray") handle_arm = sss.init("arm") if handle_arm.get_error_code() != 0: return say("failed to initialize arm") handle_sdh = sss.init("sdh") handle_base = sss.init("base") if handle_base.get_error_code() != 0: return say("failed to initialize base") # recover components handle_head = sss.recover("head") if handle_head.get_error_code() != 0: return say("failed to recover head") say("All components are ready. Please drive me around until I am localized.") return True
def __init__(self, ui, parent = None): QThread.__init__(self, parent) self.ui = ui self.mode="linear" self.add[str].connect(self.onAdd) self.rem[str].connect(self.onRem) self.sub1 = rospy.Subscriber("/chromosom/addComponent", String, self.add_callback) self.sub2 = rospy.Subscriber("/chromosom/remComponent", String, self.rem_callback) self.pub_add = rospy.Publisher('/chromosom/addComponent', String) self.pub_rem = rospy.Publisher('/chromosom/remComponent', String) ui.btnOpen.clicked.connect(self.open_gripper) ui.btnClose.clicked.connect(self.close_gripper) ui.btnOn.clicked.connect(self.power_on) ui.btnOff.clicked.connect(self.power_off) ui.btnToolChange.clicked.connect(self.tool_change) ui.rLin.clicked.connect(self.lin) ui.rDWA.clicked.connect(self.DWA) ui.btnA.clicked.connect(self.A) ui.btnB.clicked.connect(self.B) self.sss = simple_script_server()
def __init__(self): self._ros = rosHelper.ROS() self._ros.configureROS(packageName="cob_script_server") import simple_script_server self._ros.initROS() self._ss = simple_script_server.simple_script_server()
def __main__(): rospy.init_node('cob_robot_calibration_generate_cal') listener = tf.TransformListener() rospy.sleep(1) # sss = simple_script_server() # sss.move("head", "back") sss = simple_script_server() sss.move("head", "back") minimal_system = rospy.get_param('minimal_system', None) sensors_path = rospy.get_param('sensors', None) output_system = rospy.get_param('output_system', None) free_system = rospy.get_param('free_system', None) with open(sensors_path, "r") as f: sensors = yaml.load(f) pdb.set_trace() transformations = get_chains(sensors)# only interested in the sensor chains entry not the rest print sensors pdb.set_trace() transformation_dict = generate_transformation_dict( transformations, listener) print transformation_dict with open(minimal_system, "r") as f: system = yaml.load(f) pdb.set_trace() if 'transforms' in system: t = dict(transformation_dict, **system['transforms']) else: t = transformation_dict system['checkerboards'] = rospy.get_param('~checkerboards', None) if system['checkerboards'] is None: print '[ERROR]: Parameter checkerboards not found. Make sure it is set and try again' return system['transforms'] = t dh_chains = generate_chain_parameter(sensors) system['dh_chains'] = dh_chains with open(output_system, 'w') as f: f.write('####### This file is autogenerated. Do not edit #######\n') f.write(yaml.dump(system)) free = system.copy() free = settozero(free) ''' print yaml.dump(system) print "Calibration transformation suggestions are %s"%to_calib print yaml.dump(free) ''' with open(free_system, 'w') as f: f.write( '####### Use this file as template for free_0.yaml - free_2.yaml. #######\n') f.write(yaml.dump(free))
def main(): rospy.init_node(NODE) print "==> %s started " % NODE rospy.sleep(4) # service client checkerboard_checker_name = "/image_capture/visibility_check" visible = rospy.ServiceProxy(checkerboard_checker_name, Visible) rospy.wait_for_service(checkerboard_checker_name, 6) print "--> service client for for checking for chessboards initialized" kinematics_capture_service_name = "/collect_data/capture" capture_kinematics = rospy.ServiceProxy(kinematics_capture_service_name, Capture) rospy.wait_for_service(kinematics_capture_service_name, 6) print "--> service client for capture robot_states initialized" image_capture_service_name = "/image_capture/capture" capture_image = rospy.ServiceProxy(image_capture_service_name, Capture) rospy.wait_for_service(image_capture_service_name, 6) print "--> service client for capture images initialized" # init print "--> initializing sss" sss = simple_script_server() ''' sss.init("base") sss.init("torso") sss.init("head") sss.recover("base") sss.recover("torso") sss.recover("head") print "-> set joint_stiffness to 2000" stiffnessM = SetJointStiffnessRequest() #print stiffnessM.joint_stiffness stiffnessM.joint_stiffness = [1500]*7 jointstiffness_srv = rospy.ServiceProxy("/arm_controller/set_joint_stiffness",SetJointStiffness) rospy.wait_for_service("/arm_controller/set_joint_stiffness",2) print stiffnessM.joint_stiffness print jointstiffness_srv(stiffnessM) ''' print "--> setup care-o-bot for capture" sss.move("head", "back") # get position from parameter server position_path = rospy.get_param('~position_path', None) if position_path is None: print "[ERROR]: no path for positions set" return with open(position_path, 'r') as f: positions = yaml.load(f) print "==> capturing samples" start = rospy.Time.now() capture_loop(positions, sss, visible, capture_kinematics, capture_image) sss.move("arm", "calibration") print "finished after %s seconds" % (rospy.Time.now() - start).to_sec()
def init_pos(): sss = simple_script_server() sss.move("arm_right", "home") sss.move("arm_right", [[ -0.039928546971938594, 0.7617065276699337, 0.01562043859727158, -1.7979678396373568, 0.013899422367821046, 1.0327319085987252, 0.021045294231647915 ]])
def __init__(self,servers): rospy.init_node('grasp_deliver') self.sss = simple_script_server.simple_script_server() self.su = script_utils.script_utils() self.util = ScriptUtils(servers) self.sss.Init("tray") self.sss.Init("torso") self.sss.Init("arm") self.sss.Init("sdh")
def init_pos(): sss = simple_script_server() # sss.move("arm_right", [[-0.039928546971938594, 0.7617065276699337, 0.01562043859727158, -1.7979678396373568, 0.013899422367821046, 1.0327319085987252, 0.021045294231647915]]) sss.move("arm_left", [ [ 2.274847163975995, -0.08568661652370757, -0.8273207226405264, -1.9404110013940103, 1.306315205401929, 1.5627416614617742, -1.1199725164070955 ], ])
def main(): rospy.init_node(NODE) print "==> started " + NODE sss = simple_script_server() # movements print "--> moving arm to 'pregrasp'" sss.move("arm", "pregrasp") print "--> moving arm to 'calibration'" sss.move("arm", "calibration")
def __init__(self): ''' Configures the calibration node Reads configuration from parameter server or uses default values ''' # get parameter from parameter server or set defaults self.pattern_size = rospy.get_param('~pattern_size', "9x6") self.square_size = rospy.get_param('~square_size', 0.03) self.alpha = rospy.get_param('~alpha', 0.0) self.verbose = rospy.get_param('~verbose', True) self.save_result = rospy.get_param('~save_result', False) # split pattern_size string into tuple, e.g '9x6' -> tuple(9,6) self.pattern_size = tuple((int(self.pattern_size.split("x")[0]), int(self.pattern_size.split("x")[1]))) self.camera_info = CameraInfo() self.camera_info_received = False self.latest_image = Image() self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard(self.pattern_size, self.square_size) self.detector = CheckerboardDetector(self.board) self.sss = simple_script_server() topic_name = '/stereo/left/' # subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients rospy.Subscriber(topic_name + 'camera_info', CameraInfo, self.__camera_info_callback__) # subscribe to /cam3d/rgb/image_raw for image data rospy.Subscriber(topic_name + 'image_color', Image, self.__image_raw_callback__) # wait until camera informations are recieved. start_time = rospy.Time.now() while not (self.camera_info_received or rospy.is_shutdown()): rospy.sleep(0.05) if start_time + rospy.Duration(2.0) < rospy.Time.now(): # print warning every 2 seconds if the message is stil missing print "--> still waiting for /cam3d/rgb/camera_info" start_time = rospy.Time.now() # convert camera matrix an distortion coefficients and store them camera_matrix = self.camera_info.K cm = np.asarray(camera_matrix) self.cm = np.reshape(cm, (3, 3)) dist_coeffs = self.camera_info.D self.dc = np.asarray(dist_coeffs) self.frame = self.camera_info.header.frame_id
def __init__(self): ''' Configures the calibration node Reads configuration from parameter server or uses default values ''' # get parameter from parameter server or set defaults self.pattern_size = rospy.get_param('~pattern_size', "9x6") self.square_size = rospy.get_param('~square_size', 0.03) self.alpha = rospy.get_param('~alpha', 0.0) self.verbose = rospy.get_param('~verbose', True) self.save_result = rospy.get_param('~save_result', False) # split pattern_size string into tuple, e.g '9x6' -> tuple(9,6) self.pattern_size = tuple((int(self.pattern_size.split( "x")[0]), int(self.pattern_size.split("x")[1]))) self.camera_info = CameraInfo() self.camera_info_received = False self.latest_image = Image() self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard(self.pattern_size, self.square_size) self.detector = CheckerboardDetector(self.board) self.sss = simple_script_server() topic_name = '/stereo/left/' # subscribe to /cam3d/rgb/camera_info for camera_matrix and distortion coefficients rospy.Subscriber(topic_name + 'camera_info', CameraInfo, self.__camera_info_callback__) # subscribe to /cam3d/rgb/image_raw for image data rospy.Subscriber( topic_name + 'image_color', Image, self.__image_raw_callback__) # wait until camera informations are recieved. start_time = rospy.Time.now() while not (self.camera_info_received or rospy.is_shutdown()): rospy.sleep(0.05) if start_time + rospy.Duration(2.0) < rospy.Time.now(): # print warning every 2 seconds if the message is stil missing print "--> still waiting for /cam3d/rgb/camera_info" start_time = rospy.Time.now() # convert camera matrix an distortion coefficients and store them camera_matrix = self.camera_info.K cm = np.asarray(camera_matrix) self.cm = np.reshape(cm, (3, 3)) dist_coeffs = self.camera_info.D self.dc = np.asarray(dist_coeffs) self.frame = self.camera_info.header.frame_id
def __init__(self, timeout=120.0, arm_name='arm', arm_recover_srv_name='/arm_controller/lwr_node/recover'): smach.State.__init__(self, input_keys=['init_robot_goal'], output_keys=['initialised_components'], outcomes=['succeeded', 'failed']) self.timeout = timeout self.sss = simple_script_server.simple_script_server() self.arm = moveit_commander.MoveGroupCommander(arm_name) self.arm_recover_srv_name = arm_recover_srv_name self.arm_recover_client = rospy.ServiceProxy(self.arm_recover_srv_name, Empty)
def init_pos(): # Trick to move base back to odom_combined switch_controller = rospy.ServiceProxy('/base/controller_manager/switch_controller', SwitchController) print(switch_controller(None, ['odometry_controller', ], 1)) # switch off time.sleep(1.0) print(switch_controller(['odometry_controller', ], None, 1)) # switch on time.sleep(1.0) sss = simple_script_server() sss.move("arm_left", "side") # for better pics sss.move("arm_right", [[0.6849513492283021, 0.9811503738420306, -0.05053975117653131, -1.4680375957626568, -0.0824580145043452, 0.4964406318714998, -0.5817382519875354]])
def setupRobot( robotConfig, req ): rospy.loginfo( 'Preparing robot for navigation scenario ...' ) scriptServer = simple_script_server() #config = yaml.load( file( robotConfig, 'r' )) #if config: # for key, value in config.items(): # rospy.loginfo( 'Moving %s to %s' % ( key, value )) # scriptServer.move( key, value, blocking=True ) for key, value in robotConfig.items(): rospy.loginfo( 'Moving %s to %s' % ( key, value )) scriptServer.move( key, value, blocking=True ) return True, ''
def execute(self): if self.joint_goal is None: error_code = self.plan(update_ps=False) if not error_code.success: raise error_code #ToDo: in final version: use result from planning step instead of calling move_arm sss = simple_script_server.simple_script_server() if type(self.target) is str: target = self.target else: target = [list(self.joint_goal.position)] return MotionHandleSSS(sss, ('arm', target))
def main(): rospy.init_node(NODE) print "==> %s started " % NODE ## service client #checkerboard_checker_name = "/image_capture/visibility_check" #visible = rospy.ServiceProxy(checkerboard_checker_name, Visible) #rospy.wait_for_service(checkerboard_checker_name, 2) #print "--> service client for for checking for chessboards initialized" #kinematics_capture_service_name = "/collect_data/capture" #capture_kinematics = rospy.ServiceProxy( #kinematics_capture_service_name, Capture) #rospy.wait_for_service(kinematics_capture_service_name, 2) #print "--> service client for capture robot_states initialized" #image_capture_service_name = "/image_capture/capture" #capture_image = rospy.ServiceProxy(image_capture_service_name, Capture) #rospy.wait_for_service(image_capture_service_name, 2) #print "--> service client for capture images initialize # init print "--> initializing sss" sss = simple_script_server() sss.init("base") sss.init("torso") sss.init("head") sss.recover("base") sss.recover("torso") sss.recover("head") sss.move("arm_left", "home") #move to home position sss.move("arm_right", "home") #move to home position sss.move("head", "back") #move to calibration position rospy.wait_for_service("/move_group/plan_execution/set_parameters", timeout=30.0) moveit_commander.roscpp_initialize(sys.argv) # init of moveit robot = moveit_commander.RobotCommander() # init of moveit wrt robot arm_left_group = moveit_commander.MoveGroupCommander( "arm_left") # define groups arm_right_group = moveit_commander.MoveGroupCommander("arm_right") arm_left_group.set_pose_reference_frame('torso_3_link') #*** Set referece arm_right_group.set_pose_reference_frame('torso_3_link') #*** Set referece arm_list = [ arm_left_group, arm_right_group ] # list of groups for which we want to test the trajectories used for calibration #scene = moveit_commander.PlanningSceneInterface() # init if scene if collisiton is to be avoided and for the addition of the cb pattern later print "--> setup care-o-bot for capture" start = rospy.Time.now() for i in xrange(len(arm_list)): trajectory_test_loop(arm_list[i]) sss.move(arm_list[i].get_name(), "home") print "finished after %s seconds" % (rospy.Time.now() - start).to_sec() sss.move("arm_left", "home") #move to home position sss.move("arm_right", "home") #move to home position
def Start(self): self.Parse() global ah_counter ah_counter = 0 self.sss = simple_script_server.simple_script_server() self.graph = self.sss.action_handle.get_graph() rospy.loginfo("Starting <<%s>> script...", self.basename) self.Initialize() self.Run() # wait until last threaded action finishes rospy.loginfo("Wait for script to finish...") while ah_counter != 0: rospy.sleep(1) rospy.loginfo("...script finished.")
def Start(self): self.Parse() global ah_counter ah_counter = 0 self.sss = simple_script_server.simple_script_server() self.graph = self.sss.action_handle.get_graph() rospy.loginfo("Starting <<%s>> script...",self.basename) self.Initialize() self.Run() # wait until last threaded action finishes rospy.loginfo("Wait for script to finish...") while ah_counter != 0: rospy.sleep(1) rospy.loginfo("...script finished.")
def __main__(): rospy.init_node('cob_robot_calibration_generate_cal') listener = tf.TransformListener() rospy.sleep(1) sss = simple_script_server() sss.move("head", "back") minimal_system = rospy.get_param('minimal_system', None) sensors = rospy.get_param('sensors', None) output_system = rospy.get_param('output_system', None) free_system = rospy.get_param('free_system', None) z = yaml.load(open(sensors, 'r')) transformations = get_chains(z) print z transformation_dict = generate_transformation_dict( transformations, listener) print transformation_dict system = yaml.load(open(minimal_system, 'r')) if 'transforms' in system: t = dict(transformation_dict, **system['transforms']) else: t = transformation_dict system['checkerboards'] = rospy.get_param('~checkerboards', None) if system['checkerboards'] is None: print '[ERROR]: Parameter checkerboards not found. Make sure it is set and try again' return system['transforms'] = t free = system.copy() free = settozero(free) ''' print yaml.dump(system) print "Calibration transformation suggestions are %s"%to_calib print yaml.dump(free) ''' with open(output_system, 'w') as f: f.write('####### This file is autogenerated. Do not edit #######\n') f.write(yaml.dump(system)) with open(free_system, 'w') as f: f.write('####### Use this file as template for free_0.yaml - free_2.yaml. #######\n') f.write(yaml.dump(free))
def main(): rospy.init_node(NODE) print "==> %s started " % NODE # service client image_capture_service_name = "/collect_data/capture" capture = rospy.ServiceProxy(image_capture_service_name, Capture) rospy.wait_for_service(image_capture_service_name, 1) print "--> service client for capture images initialized" # init print "--> initializing sss" sss = simple_script_server() sss.init("base") sss.init("torso") sss.init("head") sss.recover("base") sss.recover("torso") sss.recover("head") print "--> setup care-o-bot for capture" sss.move("head", "back") # get position from parameter server positions_back = rospy.get_param("/script_server/arm/all_robot_back") positions_center = rospy.get_param("/script_server/arm/all_robot_center") positions_right = rospy.get_param("/script_server/arm/all_robot_right") positions_left = rospy.get_param("/script_server/arm/all_robot_left") torso_positions = ["calib_front_left2", "calib_front_right2", "calib_right2", "calib_left2", "calib_back_left2", "calib_back_right2", "home"] print "==> capturing images BACK" sss.move("torso", "back") capture_loop_arm(positions_back, sss, capture) print "==> capturing images LEFT" sss.move("torso", "left") capture_loop_arm(positions_left, sss, capture) print "==> capturing images RIGHT" sss.move("torso", "right") capture_loop_arm(positions_right, sss, capture) print "==> capturing images CENTER" sss.move("torso", "home") capture_loop_arm(positions_center, sss, capture) print "==> capturing TORSO samples" sss.move("arm", "calibration") capture_loop_torso(torso_positions, sss, capture)
def main(): rospy.init_node(NODE) print "==> %s started " % NODE rospy.sleep(4) # service client checkerboard_checker_name = "/image_capture/visibility_check" visible = rospy.ServiceProxy(checkerboard_checker_name, Visible) rospy.wait_for_service(checkerboard_checker_name, 2) print "--> service client for for checking for chessboards initialized" kinematics_capture_service_name = "/collect_data/capture" capture_kinematics = rospy.ServiceProxy( kinematics_capture_service_name, Capture) rospy.wait_for_service(kinematics_capture_service_name, 2) print "--> service client for capture robot_states initialized" image_capture_service_name = "/image_capture/capture" capture_image = rospy.ServiceProxy(image_capture_service_name, Capture) rospy.wait_for_service(image_capture_service_name, 2) print "--> service client for capture images initialized" # init print "--> initializing sss" sss = simple_script_server() sss.init("base") sss.init("torso") sss.init("head") sss.recover("base") sss.recover("torso") sss.recover("head") print "--> setup care-o-bot for capture" sss.move("head", "back") # get position from parameter server position_path = rospy.get_param('~position_path', None) if position_path is None: print "[ERROR]: no path for positions set" return with open(position_path, 'r') as f: positions = yaml.load(f) print "==> capturing samples" start = rospy.Time.now() capture_loop(positions, sss, visible, capture_kinematics, capture_image) sss.move("arm", "calibration") print "finished after %s seconds" % (rospy.Time.now() - start).to_sec()
def Parse(self): rospy.loginfo("Start parsing...") global graph global function_counter function_counter = 0 # run script in simulation mode self.sss = simple_script_server.simple_script_server(parse=True) self.Initialize() self.Run() # save graph on parameter server for further processing # self.graph = graph rospy.set_param("/script_server/graph", self.graph.string()) self.graph_pub.publish(graph.string()) rospy.loginfo("...parsing finished") function_counter = 0 return graph.string()
def main(): rospy.init_node(NODE) print "==> %s started " % NODE ## service client #checkerboard_checker_name = "/image_capture/visibility_check" #visible = rospy.ServiceProxy(checkerboard_checker_name, Visible) #rospy.wait_for_service(checkerboard_checker_name, 2) #print "--> service client for for checking for chessboards initialized" #kinematics_capture_service_name = "/collect_data/capture" #capture_kinematics = rospy.ServiceProxy( #kinematics_capture_service_name, Capture) #rospy.wait_for_service(kinematics_capture_service_name, 2) #print "--> service client for capture robot_states initialized" #image_capture_service_name = "/image_capture/capture" #capture_image = rospy.ServiceProxy(image_capture_service_name, Capture) #rospy.wait_for_service(image_capture_service_name, 2) #print "--> service client for capture images initialized" # init print "--> initializing sss" sss = simple_script_server() sss.init("base") sss.init("torso") sss.init("head") sss.recover("base") sss.recover("torso") sss.recover("head") print "--> setup care-o-bot for capture" sss.move("head", "back") # get position from parameter server position_path = rospy.get_param('position_path', None) if position_path is None: print "[ERROR]: no path for positions set" return with open(position_path, 'r') as f: positions = yaml.load(f) print "==> capturing samples" start = rospy.Time.now() capture_loop(positions, sss) print "finished after %s seconds" % (rospy.Time.now() - start).to_sec()
def execute(self, userdata): # determine target position if self.pose != '': pose = self.pose elif type(userdata.pose) is str: pose = userdata.pose elif type(userdata.pose) is list: pose = [] pose.append(userdata.pose[0]) pose.append(userdata.pose[1]) pose.append(userdata.pose[2]) else: # this should never happen userdata.message = [] userdata.message.append(5) userdata.message.append('Invalid userdata "pose"') userdata.message.append(userdata.pose) return 'failed' # try reaching pose cob_script_server = simple_script_server.simple_script_server() handle_base = cob_script_server.move('base', pose, False, self.mode) move_second = False timeout = 0 while True: move_base_state = handle_base.get_state() if (move_base_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED ) and (not move_second): # do a second movement to place the robot more exactly handle_base = cob_script_server.move('base', pose, False, self.mode) move_second = True elif (move_base_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED ) and (move_second): userdata.message = [] userdata.message.append(3) userdata.message.append('Pose was succesfully reached') return 'succeeded' elif move_base_state == actionlib_msgs.msg.GoalStatus.REJECTED or move_base_state == actionlib_msgs.msg.GoalStatus.PREEMPTED or move_base_state == actionlib_msgs.msg.GoalStatus.ABORTED or move_base_state == actionlib_msgs.msg.GoalStatus.LOST: self.speak_pub.publish('I can not reach my target position.') rospy.logerr('base movement failed with state: %d', move_base_state) return 'failed' rospy.sleep(0.1)
def __init__(self): smach.State.__init__(self, outcomes=['succeeded', 'failed'], input_keys=['place_goal'], output_keys=['place_feedback', 'place_result']) self.kinematics = kinematics.Kinematics('arm') self.move_arm = move_arm.MoveArm('arm') # distance from object center to pre-grasp self.pre_grasp_distance = rospy.get_param('~pre_grasp_distance') # distance from object to grasp self.grasp_distance = rospy.get_param('~grasp_distance') # height of the post-grasp over the object center self.post_grasp_height = rospy.get_param('~post_grasp_height') # at which angle of approach to start searching for a solution self.orbit_start_angle = 30.0 * math.pi / 180.0 rospy.loginfo('Pre-grasp distance: %f', self.pre_grasp_distance) rospy.loginfo('Grasp distance: %f', self.grasp_distance) rospy.loginfo('Post-grasp height: %f', self.post_grasp_height) self.sss = simple_script_server.simple_script_server()
def main(): rospy.init_node(NODE) print "==> %s started " % NODE # service client image_capture_service_name = "/image_capture/capture" capture = rospy.ServiceProxy(image_capture_service_name, Capture) rospy.wait_for_service(image_capture_service_name, 1) print "--> service client for capture images initialized" # init print "--> initializing sss" sss = simple_script_server() sss.init("base") sss.init("torso") sss.init("head") sss.recover("base") sss.recover("torso") sss.recover("head") print "--> setup care-o-bot for capture" sss.move("head", "back") sss.move("torso", "home") print "==> capturing images" positions = rospy.get_param("/script_server/arm/all_intrinsic") for pos in positions: print "--> moving arm to position '%s'" % pos sss.move("arm", pos) sss.sleep(1.5) print "--> capturing '%s' sample" % pos res = capture() if not res: print "--> ERROR during capture, skipping sample..." print "==> capturing finished, moving are to 'calibration' position" sss.move("arm", "calibration")
def init(): global confirm sss = simple_script_server() say("I am ready. I will initialize myself.") say("Please confirm.") confirm = False while confirm == False: rospy.sleep(0.1) # initialize components handle_head = sss.init("head") handle_torso = sss.init("torso") if handle_torso.get_error_code() != 0: return say("failed to initialize torso") handle_tray = sss.init("tray") handle_arm = sss.init("arm") if handle_arm.get_error_code() != 0: return say("failed to initialize arm") handle_sdh = sss.init("sdh") handle_base = sss.init("base") if handle_base.get_error_code() != 0: return say("failed to initialize base") # recover components handle_head = sss.recover("head") if handle_head.get_error_code() != 0: return say("failed to recover head") say("All components are ready. Please drive me around until I am localized." ) return True
def main(): rospy.init_node(NODE) print "==> %s started " % NODE # service client image_capture_service_name = "/image_capture/capture" capture = rospy.ServiceProxy(image_capture_service_name, Capture) rospy.wait_for_service(image_capture_service_name, 1) print "--> service client for capture images initialized" # init print "--> initializing sss" sss = simple_script_server() sss.init("base") sss.init("torso") sss.init("head") sss.recover("base") sss.recover("torso") sss.recover("head") print "--> setup care-o-bot for capture" sss.move("head", "back") sss.move("torso", "home") print "==> capturing images" positions = rospy.get_param("/script_server/arm/all_intrinsic") for pos in positions: print "--> moving arm to position '%s'" % pos sss.move("arm", pos) sss.sleep(1.5) print "--> capturing '%s' sample" % pos res = capture() if not res: print "--> ERROR during capture, skipping sample..." print "==> capturing finished, moving are to 'calibration' position" sss.move("arm", 'calibration')
def execute(self): if self.dryrun: return MotionHandleDummy() sss = simple_script_server.simple_script_server() return MotionHandleSSS(sss, (self.name, self.target))
def __init__(self): print "script_utils: init" self.sss = simple_script_server.simple_script_server() random.seed()
def execute(self): sss = simple_script_server.simple_script_server() return MotionHandleSSS(sss, (self.name,self.target))
import rospy import smach import smach_ros import sys, os import math from ScreenFormatting import * from autopnp_dirt_detection.srv import * from cob_phidgets.srv import SetDigitalSensor from std_msgs.msg import Bool from cob_srvs.srv import Trigger from std_srvs.srv import Empty from diagnostic_msgs.msg import DiagnosticArray from simple_script_server import simple_script_server sss = simple_script_server() class GraspCoffee(smach.State): def __init__(self): smach.State.__init__(self, outcomes=['succeeded']) def execute(self, userdata ): sf = ScreenFormat(self.__class__.__name__) grasp_out = [1.2981758976333824, -0.8947954009124528, 0.654673002423073, -1.8360340132204749, 0.3464478565208744, -1.501436942320642, -0.8940972692116552] #[1.2491146923598218, -0.9378875801441929, 0.6548824419333124, -1.8174113501016953, 0.28647834342234924, -1.501402035735602, -0.894149629089215] grasp_in = [1.319172208534874, -1.29580224985067, 0.654655549130553, -0.718045907562987, 0.2054776128372924, -0.7244338126252863, -0.5730963131848581] #inter1 = [0.8891405341359913, -0.8558920118854992, 0.737803034695563, -1.9027404972316981, -0.026581364507873642, -0.27817057618285623, -0.9220748971211243] #inter2 = [1.0291159401459364, -0.5438969547989929, 0.22078415037728266, -1.7277363331342266, 0.36641442316368955, 1.29580224985067, -0.41905355340383854] inter1 = [2.4082053019017757, -0.8928580854427391, 0.013072516097437528, -1.4938622133669865, 1.1684281210401237, -1.4554475165305913, -0.8031132553051908] inter2 = [2.5702067630718894, -0.8928406321502192, -1.5809017831639436, -1.4338403403909015, 1.569417516685821, -1.319451461215193, -0.8031132553051908]
def __init__(self): rospy.init_node('test_script') self.sss = simple_script_server.simple_script_server() rospy.Subscriber("/phidgetAO", Int16, self.phidget_callback) self.phidget_data = 0
def __init__(self): self._ros = rosHelper.ROS() self._ros.configureROS(packageName='cob_script_server') import simple_script_server self._ros.initROS() self._ss = simple_script_server.simple_script_server()
import roslib roslib.load_manifest('autopnp_tool_change') import rospy import smach import smach_ros import tf from tf.transformations import * from cob_object_detection_msgs.msg import DetectionArray, Detection from geometry_msgs import * from geometry_msgs.msg import * from simple_script_server import simple_script_server sss = simple_script_server() #from exploration_detection_cleaning import * global TOOL_WAGON_MARKER_OFFSETS TOOL_WAGON_MARKER_OFFSETS = { # todo: measure translational offsets "front": Transform( translation=Vector3(x=0.055, y=-0.975, z=0.0), rotation=Quaternion(x=-0.5, y=-0.5, z=-0.5, w=0.5) ), # quaternion_from_euler(-90.0/180.0*math.pi, -90.0/180.0*math.pi, 0.0, 'rzyx') "rear": Transform( translation=Vector3(x=0.06, y=-1.025, z=-0.46), rotation=Quaternion(x=-0.5, y=0.5, z=0.5, w=0.5) ), # quaternion_from_euler(90.0/180.0*math.pi, 90.0/180.0*math.pi, 0.0, 'rzyx')
def init_pos(): sss = simple_script_server() # sss.move("arm_right", [[-0.039928546971938594, 0.7617065276699337, 0.01562043859727158, -1.7979678396373568, 0.013899422367821046, 1.0327319085987252, 0.021045294231647915]]) sss.move("arm_left", [[-0.17566952192977148, 0.6230441162870415, 0.27476319388433623, -1.1163222472275676, 0.3166667900974902, 0.5333301416131739, -0.32441227738211875], ])
def __init__(self): rospy.init_node('test_script') self.sss = simple_script_server.simple_script_server()
def main(): rospy.init_node(NODE) print "==> %s started " % NODE rospy.wait_for_service("/move_group/plan_execution/set_parameters",timeout=30.0) chessboard_pose = rospy.Publisher('/cob_calibration/chessboard_pose', geometry_msgs.msg.PoseStamped, queue_size=1) print 'chessboard_pose publisher activated' moveit_commander.roscpp_initialize(sys.argv) # init of moveit robot = moveit_commander.RobotCommander()# init of moveit wrt robot # scene = moveit_commander.PlanningSceneInterface() # init if scene if collisiton is to be avoided and for the addition of the cb pattern later print "============ Robot Groups:" print robot.get_group_names() print "============" listener = tf.TransformListener() rospy.sleep(1.5) # torso_ik = rospy.ServiceProxy('/lookat_get_ik', GetPositionIK) # no Torso link # init print "--> initializing sss" sss = simple_script_server() sss.init("base") sss.init("torso") sss.init("head") sss.recover("base") sss.recover("torso") sss.recover("head") sss.move("arm_left","home") sss.move("arm_right","home") print "--> components initialized" #camera_link_left = "/torso_cam3d_left_link" #renamed #camera_link_right = "/torso_cam3d_right_link" #new addition #[xhead, yhead, zhead] = get_position(listener, camera_link_left)[0] #print "Positon wrt torso_cam3d_left" #print xhead, yhead, zhead #[xhead, yhead, zhead] = get_position(listener, camera_link_right)[0] #print "Positon wrt torso_cam3d_left" #print xhead, yhead, zhead print "--> setup care-o-bot for capture" # arm_left_group = moveit_commander.MoveGroupCommander("arm_left") # define groups # arm_right_group = moveit_commander.MoveGroupCommander("arm_right") # arm_left_group.set_pose_reference_frame('torso_3_link')#*** # arm_right_group.set_pose_reference_frame('torso_3_link')#*** # arm_list = [arm_left_group,arm_right_group] # arm_planning_reference_frames = ['torso_cam3d_left_link','torso_cam3d_right_link'] # orientation_mean = [[-0.55194, -0.83389, 0, 0], # [0.83389, 0.55194, 0, 0]] # limits = [ # [[0.15074, 0.40, 0.52078-0.698747/2], #-0.457/2.0 # [0.6808, -0.40, 1.135-0.698747/2]], # [[0.15074, 0.40, 0.52078-0.698747/2], #-0.457/2.0 # [0.6808, -0.40, 1.135-0.698747/2]] # ] # discritization = [[2,2,2], # [2,2,2]] try: arms = rospy.get_param('/cob_calibration_executive/arms') except: raise NameError('Couldn\'t find the configuration parameters under \'~cob_calibration/arms\' namespace.') config_list = [] for k in arms.keys(): config_list.append(arms[k]) arm_names = [] limits = [] discritization = [] orientation_mean = [] arm_planning_reference_frames = [] for i, arm in enumerate(config_list): arm_names.append(arm['name']) l1 = arm['limits_corner_1'] l2 = arm['limits_corner_2'] l1 = [float(eval(str(i))) for i in l1] # convert the limit values into float l2 = [float(eval(str(i))) for i in l2] limits.append([l1, l2]) d = map(int, arm['discritization']) discritization.append(d) h = arm['hand_orientation'] h = [float(eval(str(i))) for i in h] # convert the orientation values into float orientation_mean.append(h) arm_planning_reference_frames.append(str(arm['arm_planning_reference_frame'])) arm_list = [] for i, arm in enumerate(arm_names): arm_list.append(moveit_commander.MoveGroupCommander(arm)) f = arm_planning_reference_frames[i] arm_list[i].set_pose_reference_frame(f) trajectory_joint_angles = [] # the first element of each entry is the Trajectory list for all successful calibration points while the second is the list of finla joint angles corresponding to that list file_path = rospy.get_param('~output_path', None) directory = os.path.dirname(file_path) if file_path is not None: if not os.path.exists(directory): os.makedirs(directory) #pdb.set_trace() for i in xrange(len(arm_list)): trajectory_joint_angles.append(generate_calibration_trajectory( arm_list[i], limits[i], discritization[i], orientation_mean[i], arm_planning_reference_frames[i], False, chessboard_pose)) file_path_group_angles = file_path+arm_list[i].get_name()+"calibration_positions.yaml" with open(file_path_group_angles, 'w') as f: f.write('# autogenerated: Do not edit #\n') f.write(yaml.dump(trajectory_joint_angles[i][1]))# this is the second entry of the element in the list corresponding to the joint angles for all successful calibration points file_path_group_trajectories = file_path+arm_list[i].get_name()+"calibration_trajectories.yaml" with open(file_path_group_trajectories, 'w') as f: f.write('# autogenerated: Do not edit #\n') f.write(yaml.dump(trajectory_joint_angles[i][0])) #this is the first entry of the element in the list corresponding to the joint trajectories for all successful calibration points display_string = '%s ik solutions found for '% len(trajectory_joint_angles[i][0]) display_string = display_string + arm_list[i].get_name() print (display_string) file_path_cb_positions = file_path+arm_list[i].get_name()+"_cb_positions.yaml" with open(file_path_cb_positions, 'w') as f: f.write('# autogenerated: Do not edit #\n') trajectory_joint_angles[i][2].append(arm_planning_reference_frames[i]) f.write(yaml.dump(trajectory_joint_angles[i][2]))
def main(): rospy.init_node(NODE) print "==> %s started " % NODE chessboard_pose = rospy.Publisher( '/cob_calibration/chessboard_pose', PoseStamped) print 'chessboard_pose publisher activated' listener = tf.TransformListener() rospy.sleep(1.5) arm_ik = rospy.ServiceProxy('/compute_ik', GetPositionIKMoveit) torso_ik = rospy.ServiceProxy('/lookat_get_ik', GetPositionIK) ''' (t,r)=listener.lookupTransform('/arm_0_link','arm_7_link',rospy.Time(0)) a=calculate_ik((t,r), arm_ik) print a[0] ''' # init print "--> initializing sss" sss = simple_script_server() sss.init("base") sss.init("torso") sss.init("head") sss.recover("base") sss.recover("torso") sss.recover("head") camera_link = "/cam_reference_link" [xhead, yhead, zhead] = get_position(listener, camera_link)[0] print xhead, yhead, zhead print "--> setup care-o-bot for capture" sss.move("head", "back") calibration_seed = rospy.get_param("/script_server/arm/calibration") #sss.move("arm",[a[0]]) nextPose = PoseStamped() torso = TorsoIK() # define cuboid for positions # limits from base_link frame limits = {'x': (-0.4, -1.0), 'y': (-0.3, 0.3), 'z': (0.5, 1.5)} sample_density = {'x': 6, 'y': 6, 'z': 6} sample_positions = {'x': [], 'y': [], 'z': []} for key in limits.keys(): limits[key] = sorted(limits[key]) sample_positions[key].append(limits[key][0]) diff = limits[key][1] - limits[key][0] step = 1.0 * diff / (sample_density[key] - 1) # print key, ' ',diff,' ',step while sample_positions[key][-1] + step <= (limits[key][1] + 0.01): sample_positions[key].append(sample_positions[key][-1] + step) joint_states = [] torso.get_torso_limits() cb_links = ["/chessboard_center","/chessboard_lu_corner", "/chessboard_ru_corner", "/chessboard_ll_corner", "/chessboard_rl_corner"] for x in sample_positions['x']: for y in sample_positions['y']: for z in sample_positions['z']: #for q in quaternion: for cb_link in cb_links: print "\033[1;34mNew Position\033[1;m" nextPose.header.frame_id = '/base_link' nextPose.pose.position.x = x nextPose.pose.position.y = y nextPose.pose.position.z = z # (0,0,0,1) for cob3-6 nextPose.pose.orientation.x = 0 nextPose.pose.orientation.y = 0 nextPose.pose.orientation.z = 0 nextPose.pose.orientation.w = 1 chessboard_pose.publish(nextPose) rospy.sleep(0.2) transformation_base_cb = get_position( listener, '/chessboard_center') [x1, y1, z1] = transformation_base_cb[0] (dx, dy, dz) = (x1 - xhead, y1 - yhead, z1 - zhead) roll = 0 pitch = math.atan2(dz, -dx) yaw = -math.atan2(dy, -dx) ''' Add noise to roll pitch and yaw values of cb ''' std_dev = 0.3 roll = np.random.normal(roll,std_dev,1)[0] pitch = np.random.normal(pitch,std_dev,1)[0] yaw = np.random.normal(yaw,std_dev,1)[0] q=tf.transformations.quaternion_from_euler(roll, pitch, yaw) #print q nextPose.pose.orientation.x = q[0] nextPose.pose.orientation.y = q[1] nextPose.pose.orientation.z = q[2] nextPose.pose.orientation.w = q[3] chessboard_pose.publish(nextPose) rospy.sleep(0.2) transformation_base_cb = get_position( listener, cb_link) #if not torso.in_range(angles): #continue #print t try: torso_js = lookat(transformation_base_cb ,torso_ik)[:len(torso.limits)] except: break if torso_js[0] is None: break if not torso.valid_ik(torso_js): break print '\033[1;33mTorso solution found\033[1;m' #(t, r) = get_cb_pose(listener, '/head_cam3d_link') (t, r) = get_cb_pose(listener, '/base_link') js = calculate_ik(( t, r), arm_ik, calibration_seed[0]) if js[0] is not None: joint_states.append({'joint_position': js[0] , 'torso_position': list(torso_js)}) print joint_states[-1] print '\033[1;32mIK solution found\033[1;m' else: print '\033[1;31mNo solution found\033[1;m' path = rospy.get_param('~output_path', None) directory = os.path.dirname(path) if path is not None: if not os.path.exists(directory): os.makedirs(directory) with open(path, 'w') as f: f.write('# autogenerated: Do not edit #\n') f.write(yaml.dump(joint_states)) else: print yaml.dump(joint_states) print '%s ik solutions found' % len(joint_states)
def execute(self): if self.dryrun: return MotionHandleDummy() sss = simple_script_server.simple_script_server() return MotionHandleSSS(sss, (self.name,self.target))
def main(): rospy.init_node(NODE) print "==> %s started " % NODE chessboard_pose = rospy.Publisher( '/cob_calibration/chessboard_pose', PoseStamped) print 'chessboard_pose publisher activated' listener = tf.TransformListener() rospy.sleep(1.5) arm_ik = rospy.ServiceProxy('/cob_ik_wrapper/arm/get_ik', GetPositionIK) ''' (t,r)=listener.lookupTransform('/arm_0_link','arm_7_link',rospy.Time(0)) a=calculate_ik((t,r), arm_ik) print a[0] ''' # init print "--> initializing sss" sss = simple_script_server() sss.init("base") sss.init("torso") sss.init("head") sss.recover("base") sss.recover("torso") sss.recover("head") print "--> setup care-o-bot for capture" sss.move("head", "back") calibration_seed = rospy.get_param("/script_server/arm/calibration") #sss.move("arm",[a[0]]) nextPose = PoseStamped() torso = TorsoIK() torso.set_camera_viewfield(rospy.get_param('~camera_view_angle')) # lissajous like figure for rotation cb_tip_offset = 3 cb_tip_positions = [(0, 0), (1, 0), (0, 1), (-1, 0), (0, -1)] quaternion = [] for cb_tip_p in cb_tip_positions: temp = list(cb_tip_p) temp.append(cb_tip_offset) vector_to = np.matrix(temp) vector_from = np.matrix([0, 0, 1]) a = vector_to + vector_from a = a / np.linalg.norm(a) #print a ''' #print '*'*20 print type(a) print a.T.shape print type(vector_from) print vector_from.T.shape ''' w = np.dot(vector_from, a.T) vector_from = vector_from.tolist()[0] a = a.tolist()[0] #print vector_from #print a x = vector_from[1] * a[2] - vector_from[2] * a[1] y = vector_from[2] * a[0] - vector_from[0] * a[2] z = vector_from[0] * a[1] - vector_from[1] * a[0] quaternion.append(tuple(tf.transformations.quaternion_multiply( [0, 0, 0, 1], [x, y, z, w]).reshape(1, 4).tolist()[0])) # define cuboid for positions # limits from base_link frame limits = {'x': (-0.5, -1.2), 'y': (-0.3, 0.3), 'z': (0.5, 1.0)} sample_density = {'x': 5, 'y': 5, 'z': 5} sample_positions = {'x': [], 'y': [], 'z': []} for key in limits.keys(): limits[key] = sorted(limits[key]) sample_positions[key].append(limits[key][0]) diff = limits[key][1] - limits[key][0] step = 1.0 * diff / (sample_density[key] - 1) # print key, ' ',diff,' ',step while sample_positions[key][-1] + step <= (limits[key][1] + 0.01): sample_positions[key].append(sample_positions[key][-1] + step) joint_states = [] for x in sample_positions['x']: for y in sample_positions['y']: for z in sample_positions['z']: for q in quaternion: nextPose.header.frame_id = '/base_link' nextPose.pose.position.x = x nextPose.pose.position.y = y nextPose.pose.position.z = z # (0,0,0,1) for cob3-6 nextPose.pose.orientation.x = q[0] nextPose.pose.orientation.y = q[1] nextPose.pose.orientation.z = q[2] nextPose.pose.orientation.w = q[3] chessboard_pose.publish(nextPose) rospy.sleep(0.2) (t, r) = get_cb_pose_head( listener, '/head_color_camera_l_link') angles = calculate_angles(t) if not torso.in_range(angles): continue #print t #(t, r) = get_cb_pose(listener, '/head_cam3d_link') #print t (t, r) = get_cb_pose(listener, '/arm_0_link') js = calculate_ik(( t, r), arm_ik, calibration_seed[0]) if js[0] is not None: print 'IK solution found' else: continue torso_state = [-a for a in torso.calculate_ik(angles)] for torso_js in [torso_state, [0] * len(torso_state)]: joint_states.append({'joint_position': js[ 0], 'torso_position': list(torso_js)}) print joint_states[-1] path = rospy.get_param('~output_path', None) directory = os.path.dirname(path) if path is not None: if not os.path.exists(directory): os.makedirs(directory) with open(path, 'w') as f: f.write('# autogenerated: Do not edit #\n') f.write(yaml.dump(joint_states)) else: print yaml.dump(joint_states) print '%s ik solutions found' % len(joint_states)