def _test_relative_position_control(posn, botname, base_controller, base_planner, close_loop, smooth, trans_thresh, angular_thresh): bot = Robot(botname, base_config={ 'base_controller': base_controller, 'base_planner': base_planner }, use_arm=False, use_camera=False) start_state = np.array(bot.base.get_state('odom')) desired_target = _get_absolute_pose(posn, start_state) bot.base.go_to_relative(posn, use_map=False, close_loop=close_loop, smooth=smooth) end_state = np.array(bot.base.get_state('odom')) if False: # Plot results to file file_name = '{:s}_position_{:s}_controller_{:s}_planner_{:s}_close{:d}_smooth{:d}-{:s}.pdf' posn_str = ['{:0.1f}'.format(x) for x in posn] file_name = file_name.format(botname, ','.join(posn_str), base_controller, base_planner, int(close_loop), int(smooth), _get_time_str()) tmp_dir = os.path.join(os.path.dirname(__file__), 'tmp') _mkdir_if_missing(tmp_dir) file_name = os.path.join(tmp_dir, file_name) bot.base.controller.plot_plan_execution(file_name) dist = np.linalg.norm(end_state[:2] - desired_target[:2]) angle = end_state[2] - desired_target[2] angle = np.abs(wrap_theta(angle)) assert dist < trans_thresh assert angle * 180. / np.pi < angular_thresh
def test_setUp(self): Util.init_ChromeDriver(self) #Util.init_IE_Driver(self) self.driver.maximize_window() self.driver.get('http://192.168.0.79') yield self.driver.switch_to_default_content() sleep(2) self.driver.find_element_by_id('system').click() #self.driver.find_element_by_id('system').click() sleep(2) robot = Robot() robot.add_to_clipboard("admin") robot.paste() sleep(1) robot.press_and_release(Keys.tab) sleep(1) robot.add_to_clipboard("iepl") robot.paste() sleep(1) robot.press_and_release(Keys.enter) self.driver.switch_to_frame("center") wait = WebDriverWait(self.driver, 10) wait.until( expected_conditions.visibility_of_element_located( (By.XPATH, "//*[@id='pwpForm']//input[@value='off']"))) self.driver.find_element_by_xpath( "//*[@id='pwpForm']//input[@value='off']").click() self.setPassword("iepl", "admin") self.driver.quit()
def main(): np.set_printoptions(precision=4, suppress=True) bot = Robot('locobot') print('\nInitial base pose:') ini_base_pose = bot.base.base_state.vslam.base_pose_xyyaw if None in ini_base_pose: raise RuntimeError('No camera pose data received!!!\n' 'Please check if ORB-SLAM2 is running properly\n' 'and make sure the tracking is not lost!') print('X: {0:f} Y: {1:f} Yaw: {2:f}'.format(ini_base_pose[0], ini_base_pose[1], ini_base_pose[2])) print('\nInitial camera pose:') ini_c_trans, ini_c_rot, ini_c_T = bot.base.base_state.vslam.camera_pose print('Translation:', ini_c_trans) print('Rotation:') print(ini_c_rot) bot.base.set_vel(fwd_speed=0.1, turn_speed=0, exe_time=1) bot.base.stop() print('\nBase pose after moving:') af_base_pose = bot.base.base_state.vslam.base_pose_xyyaw print('X: {0:f} Y: {1:f} Yaw: {2:f}'.format(af_base_pose[0], af_base_pose[1], af_base_pose[2])) print('\nCamera pose after moving:') af_c_trans, af_c_rot, aft_c_T = bot.base.base_state.vslam.camera_pose print('Translation:', af_c_trans) print('Rotation:') print(af_c_rot)
def test_firmware(self): self.driver.find_element_by_xpath("//*[@id='fwupdate']/a").click() self.driver.switch_to_frame("center") wait = WebDriverWait(self.driver, 10) wait.until( expected_conditions.visibility_of_element_located( (By.XPATH, "//*[@id='fwupdateForm']/p[2]/input"))) self.driver.find_element_by_xpath( "//*[@id='fwupdateForm']/p[2]/input").click() sleep(1) robot = Robot() robot.add_to_clipboard("I:\Share\Umesh\Firmware\DTE\dte101_1.2.6.nxf") robot.paste() robot.press_and_release(Keys.enter) self.driver.find_element_by_xpath("//input[@type='submit']").click() wait.until( expected_conditions.visibility_of_element_located( (By.XPATH, "//*[@id='contentIframe']/p[1]/b"))) self.assertEqual( self.driver.find_element_by_xpath( "//*[@id='contentIframe']/p[1]/b").text, "Firmware transfer and update succeeded!") sleep(1) self.driver.find_element_by_xpath("//input[@type='checkbox']").click() self.driver.find_element_by_xpath("//input[@type='submit']").click() wait.until( expected_conditions.visibility_of_element_located( (By.XPATH, "//*[@id='contentIframe']/p[1]"))) self.assertEqual( self.driver.find_element_by_xpath( "//*[@id='contentIframe']/p[1]").text, "The device was successfully restarted.") self.verify_settings()
def main(): bot = Robot("kinect2") vis = open3d.Visualizer() vis.create_window("3D Map") pcd = open3d.PointCloud() coord = open3d.create_mesh_coordinate_frame(1, [0, 0, 0]) vis.add_geometry(pcd) vis.add_geometry(coord) # We update the viewer every a few seconds update_time = 4 while True: start_time = time.time() pts, colors = bot.camera.get_current_pcd() pcd.clear() # note that open3d.Vector3dVector is slow pcd.points = open3d.Vector3dVector(pts) pcd.colors = open3d.Vector3dVector(colors / 255.0) vis.update_geometry() vis.poll_events() vis.update_renderer() s_t = time.time() while time.time() - s_t < update_time: vis.poll_events() vis.update_renderer() time.sleep(0.1) end_time = time.time() process_time = end_time - start_time print("Updating every {0:.2f}s".format(process_time))
def main(): # Example poses # orientation can be either rotation matrix or quaternion target_poses = [ { "position": np.array([-0.38293327, 0.5077687, 0.72630546]), "orientation": np.array( [ [-0.88077548, -0.45764594, -0.12163366], [-0.14853923, 0.02311451, 0.98863634], [-0.44963391, 0.88883402, -0.08833707], ] ), }, ] bot = Robot( "ur5", use_arm=True, use_base=False, use_camera=False, use_gripper=False ) bot.arm.move_to_neutral() time.sleep(1) for pose in target_poses: bot.arm.set_ee_pose(plan=True, **pose) time.sleep(1) bot.arm.go_home() # Forward Kinematics Example angles = np.array([1.9278, -0.9005, -0.002, -1.0271, -0.5009, 0.5031]) print(bot.arm.compute_fk_position(angles, "ee_link"))
def main(): config = dict(moveit_planner="ESTkConfigDefault") bot = Robot( "sawyer", use_arm=True, use_base=False, use_camera=False, use_gripper=True, arm_config=config, ) obstacle_handler = MoveitObjectHandler() # Add a table # position and orientation (quaternion: x, y, z, w) of the table pose = [0.8, 0.0, -0.1, 0.0, 0.0, 0.0, 1.0] # size of the table (x, y, z) size = (1.35, 2.0, 0.1) obstacle_handler.add_table(pose, size) target_pos = np.array([0.45251711, 0.16039618, 0.08021886]) target_ori = np.array( [ [-0.00437824, 0.99994626, 0.00939675], [0.99998735, 0.0044013, -0.00243524], [-0.00247647, 0.00938597, -0.99995288], ] ) bot.arm.go_home() time.sleep(1) traj = bot.arm.make_plan_pose(target_pos, target_ori) time.sleep(1) print(traj) for joints in traj: bot.arm.set_joint_positions(joints, plan=False) time.sleep(1)
def main(_): robot = Robot('husky', use_arm=False, use_camera=False, use_gripper=False, base_config={ 'base_controller': FLAGS.base_controller, 'base_planner': FLAGS.base_planner }) # linear_velocity in m/s linear_velocity = 1.0 # rotational_velocity in radian / s rotational_velocity = 0.5 # execution_time in seconds execution_time = 1 i = 0 while (i < 100): # Command to execute motion robot.base.set_vel(fwd_speed=linear_velocity, turn_speed=rotational_velocity, exe_time=execution_time) print(robot.base.get_state('odom')) bot.base.stop()
def main(): parser = argparse.ArgumentParser(description="Argument Parser") parser.add_argument("--floor_height", type=float, default=0.03, help="the z coordinate of the floor") parser.add_argument( "--gripper_len", type=float, default=0.12, help="the gripper length (length from gripper_link" "to the tip of the gripper", ) args = parser.parse_args() np.set_printoptions(precision=4, suppress=True) bot = Robot("locobot", use_arm=True, use_base=False, use_gripper=True) bot.gripper.close() bot.camera.set_pan_tilt(0, 0.7, wait=True) bot.arm.go_home() while True: try: push(bot, z_lowest=args.floor_height, gripper_length=args.gripper_len) except: pass time.sleep(1)
def main(): # Example poses # orientation can be either rotation matrix or quaternion target_poses = [ { 'position': np.array([0.8219, 0.0239, 0.0996]), 'orientation': np.array([[-0.3656171, 0.6683861, 0.6477531], [0.9298826, 0.2319989, 0.2854731], [0.0405283, 0.7067082, -0.7063434]]) }, { 'position': np.array([0.7320, 0.1548, 0.0768]), 'orientation': np.array([0.1817, 0.9046, -0.1997, 0.3298]) }, ] bot = Robot('sawyer', use_arm=True, use_base=False, use_camera=False, use_gripper=True) bot.arm.go_home() time.sleep(1) for pose in target_poses: bot.arm.set_ee_pose(plan=True, **pose) time.sleep(1) bot.arm.go_home()
def main(): np.set_printoptions(precision=4, suppress=True) bot = Robot('locobot') bot.arm.go_home() displacement = np.array([0, 0, -0.15]) bot.arm.move_ee_xyz(displacement, plan=True) bot.arm.go_home()
def __init__(self, z_lower_treshold, z_upper_treshold, x_min, y_min, x_max, y_max): self.bot = Robot("locobot", base_config={"base_planner": "none"}) self.ocGrid = None # self.points = None # these are the points from orbslam self.colors = None self.z_lower_treshold = z_lower_treshold self.z_upper_treshold = z_upper_treshold # occupancy grid params self.xMax = None self.xMin = None self.yMin = None self.yMax = None # grid self.grid = OccupancyGrid() self.xCells = None self.yCells = None self.x_min = x_min self.x_max = x_max self.y_min = y_min self.y_max = y_max self.initGrid()
def main(_): bot = Robot( FLAGS.botname, base_config={ "base_controller": FLAGS.base_controller, "base_planner": FLAGS.base_planner, }, ) posn = np.asarray(FLAGS.relative_position, dtype=np.float64, order="C") bot.base.go_to_relative(posn, use_map=True, close_loop=FLAGS.close_loop, smooth=FLAGS.smooth) if hasattr(bot.base.controller, "plot_plan_execution"): posn = FLAGS.relative_position posn_str = ",".join(posn) file_name = os.path.join( "position_{:s}_close{:d}_smooth{:d}-{:s}.pdf".format( posn_str, int(FLAGS.close_loop), int(FLAGS.smooth), get_time_str())) tmp_dir = os.path.join(os.path.dirname(__file__), "tmp") if not os.path.exists(tmp_dir): os.makedirs(tmp_dir) file_name = os.path.join(tmp_dir, file_name) print("Trajectory saved to:", file_name) bot.base.plot_plan_execution(file_name) bot.base.stop()
def main(): parser = argparse.ArgumentParser( description='log trajectory csv file to control robot joint state') parser.add_argument('path', type=str, help="input trajectory csv file") parser.add_argument('--fps', type=int, default=10, help="log csv fps") parser.add_argument('--ofps', type=int, default=1, help="output fps") args = parser.parse_args() target_joints = [] with open(args.path) as csvfile: rows = csv.reader(csvfile) for row in rows: target_joints.append(row) bot = Robot('locobot') bot.arm.go_home() for joint in target_joints[1:-1:args.fps / args.ofps]: bot.arm.set_joint_positions([float(x) for x in joint[0:5]], plan=False) gripper_value = float(joint[5]) if gripper_value > -0.024: bot.gripper.close() else: bot.gripper.open() bot.arm.go_home()
def main(): # Example poses # orientation can be either rotation matrix or quaternion target_poses = [ { "position": np.array([0.279, 0.176, 0.217]), "orientation": np.array([ [0.5380200, -0.6650449, 0.5179283], [0.4758410, 0.7467951, 0.4646209], [-0.6957800, -0.0035238, 0.7182463], ]), }, { "position": np.array([0.339, 0.0116, 0.255]), "orientation": np.array([0.245, 0.613, -0.202, 0.723]), }, ] bot = Robot("locobot") bot.arm.go_home() for pose in target_poses: bot.arm.set_ee_pose(**pose) time.sleep(1) bot.arm.go_home()
def main(): # Example poses # orientation can be either rotation matrix or quaternion target_poses = [ { "position": np.array([0.18, 0.0, 0.15]), "orientation": np.array([0, 1.57, 0]), }, ] bot = Robot("vx300s", use_arm=True, use_base=False, use_camera=False, use_gripper=False) bot.arm.go_home() time.sleep(1) for pose in target_poses: bot.arm.set_ee_pose(plan=True, **pose) time.sleep(2) bot.arm.go_home() # Forward Kinematics Example angles = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) print(bot.arm.compute_fk_position(angles, "vx300s/ee_arm_link"))
def camera_checkout(input_args): robot = Robot('locobot_lite') # Should be center and horizontal robot.camera.reset() print('Camera state [pan, tilt]: {}'.format(robot.camera.state)) show_camera_output(robot) # Should be center and pointed down (viewing the arm) robot.camera.set_pan_tilt(0., 0.5) print('Camera state [pan, tilt]: {}'.format(robot.camera.state)) show_camera_output(robot) # Should be pointed to the left and down robot.camera.set_pan_tilt(1., 0.3) print('Camera state [pan, tilt]: {}'.format(robot.camera.state)) show_camera_output(robot) # Should be pointed to the right and up robot.camera.set_pan_tilt(-1., -0.3) print('Camera state [pan, tilt]: {}'.format(robot.camera.state)) show_camera_output(robot) robot.camera.reset()
def main(): parser = argparse.ArgumentParser(description="Argument Parser") parser.add_argument("--floor_height", type=float, default=0.03, help="the z coordinate of the floor") args = parser.parse_args() np.set_printoptions(precision=4, suppress=True) bot = Robot("locobot") bot.camera.set_pan_tilt(0, 0.7, wait=True) bot.arm.go_home() bot.arm.set_joint_positions([1.96, 0.52, -0.51, 1.67, 0.01], plan=False) vis = open3d.Visualizer() vis.create_window("3D Map") pcd = open3d.PointCloud() coord = open3d.create_mesh_coordinate_frame(1, [0, 0, 0]) vis.add_geometry(pcd) vis.add_geometry(coord) while True: pts, colors = bot.camera.get_current_pcd(in_cam=False) pts, colors = filter_points(pts, colors, z_lowest=args.floor_height) pcd.clear() # note that open3d.Vector3dVector is slow pcd.points = open3d.Vector3dVector(pts) pcd.colors = open3d.Vector3dVector(colors / 255.0) vis.update_geometry() vis.poll_events() vis.update_renderer() time.sleep(0.1)
def main(_): bot = Robot(FLAGS.botname, use_arm=False, use_camera=False, use_gripper=False, base_config={ 'base_controller': FLAGS.base_controller, 'base_planner': FLAGS.base_planner }) posn = np.asarray(FLAGS.relative_position, dtype=np.float64, order='C') bot.base.controller.goto(posn) # if hasattr(bot.base.controller, 'plot_plan_execution'): # posn = FLAGS.relative_position # posn_str = ','.join(posn) # file_name = 'position_{:s}_controller_{:s}_close{:d}_smooth{:d}-{:s}.pdf' # file_name = file_name.format(posn_str, FLAGS.base_controller, # int(FLAGS.close_loop), int(FLAGS.smooth), get_time_str()) # tmp_dir = os.path.join(os.path.dirname(__file__), 'tmp') # if not os.path.exists(tmp_dir): # os.makedirs(tmp_dir) # file_name = os.path.join(tmp_dir, file_name) # rospy.loginfo('Trajectory saved to %s.', file_name) # bot.base.controller.plot_plan_execution(file_name) bot.base.stop()
def __init__(self, ros_ws_abspath, ros_ws_src_path): rospy.logdebug("======= In LocobotEnv") # Object controls all objects' positions !!! #self.obj_positions = Object_Position() self.robot_name_space = "" super(LocoBotEnv, self).__init__(robot_name_space=self.robot_name_space) rospy.logdebug("Goint to create a PyRobot Instance") self.robot = Robot('locobot') rospy.logdebug("PyRobot Instance created") # We start all the ROS related Subscriber and Publishers self.JOINT_STATES_SUBSCRIBE_TOPIC = '/joint_states' self.joint_names = [ "joint_1", "joint_2", "joint_3", "joint_1", "joint_4", "joint_5", "joint_6", "joint_7" ] self._check_all_systems_ready() self.joint_states_sub = rospy.Subscriber( self.JOINT_STATES_SUBSCRIBE_TOPIC, JointState, self.joints_callback) self.joints = JointState() # Wait until LocoBot reached its StartUp positions #self.wait_locobot_ready() rospy.logdebug("======= Out LocobotEnv")
def create_robot(): # Please change this to match your habitat_sim repo's path path_to_habitat_scene = os.path.dirname(os.path.realpath(__file__)) relative_path = "../examples/habitat/scenes/skokloster-castle.glb" common_config = dict(scene_path=os.path.join(path_to_habitat_scene, relative_path)) bot = Robot("habitat", common_config=common_config) return bot
def main(): # Example torque [it should be a list of 4 elements # with values in the range of (-4.1:4.1)] target_torques = 4 * [0.1] arm_config = dict(control_mode='torque') bot = Robot('locobot', arm_config=arm_config) bot.arm.set_joint_torques(target_torques)
def test_make_robot_base(botname, base_controller, base_planner): bot = Robot(botname, base_config={ 'base_controller': base_controller, 'base_planner': base_planner }, use_arm=False, use_camera=False) return bot
def main(_): bot = Robot( FLAGS.botname, base_config={ "base_controller": FLAGS.base_controller, "base_planner": "none" }, ) states = get_trajectory(bot, FLAGS.type) bot.base.track_trajectory(states, close_loop=FLAGS.close_loop)
def main(_): bot = Robot(FLAGS.botname, use_base=True, use_arm=False, use_camera=False, use_gripper=False) bot.base.set_vel(fwd_speed=FLAGS.linear_speed, turn_speed=FLAGS.angular_speed, exe_time=FLAGS.duration) bot.base.stop()
def main(): bot = Robot('ur', use_arm=True, use_base=False, use_camera=False, use_gripper=False) bot.arm.go_home() time.sleep(1) bot.arm.move_to_neutral() time.sleep(1)
def test_vel_cmd_v(botname, base_controller, base_planner): bot = Robot(botname, base_config={'base_controller': base_controller, 'base_planner': base_planner}, use_arm=False, use_camera=False) state_before = np.array(bot.base.get_state('odom')) bot.base.set_vel(fwd_speed=0.2, turn_speed=0.0, exe_time=2) bot.base.stop() state_after = np.array(bot.base.get_state('odom')) dist = np.linalg.norm(state_after[:2] - state_before[:2]) assert 0.3 < dist < 0.5
def main(_): bot = Robot(FLAGS.botname, use_arm=False, base_config={ 'base_planner': FLAGS.base_planner, 'base_controller': FLAGS.base_controller }) bot.base.set_vel(fwd_speed=FLAGS.linear_speed, turn_speed=FLAGS.angular_speed, exe_time=FLAGS.duration) bot.base.stop()
def main(): arm = Robot("vx300s", use_arm=True, use_base=False, use_camera=False, use_gripper=True) arm.gripper.close() arm.gripper.open() arm.gripper.set_gripper_pressure(0.8) arm.gripper.close() arm.gripper.open()
def __init__(self, botname): """ Constructor, sets up arm and active camera. """ self.bot = Robot(botname) self.bot.camera = CalibrationCamera(self.bot.configs) self.bot.camera.reset() # self.bot.arm.go_home() # Initialize listening for transforms self.listener = tf.TransformListener() rospy.sleep(1)