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():
bot = Robot('locobot')
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.base.base_state.vslam.get_3d_map()
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.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 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")
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(_):
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 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 __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():
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.,1.]
# 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():
# 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():
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, use_sim=True)
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(_):
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():
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 _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 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 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 __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 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():
# 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 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 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_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 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():
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 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 __init__(self):
base_config_dict = {'base_controller': 'proportional'}
arm_config_dict = dict(moveit_planner='ESTkConfigDefault')
self._robot = Robot("locobot",
use_base=True,
use_arm=True,
use_camera=True,
base_config=base_config_dict,
arm_config=arm_config_dict)
self._done = True
self._speaker = Speaker()
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)
def main():
target_joints = [[0.408, 0.721, -0.471, -1.4, 0.920],
[-0.675, 0, 0.23, 1, -0.70]]
bot = Robot('locobot')
bot.arm.go_home()
for joint in target_joints:
bot.arm.set_joint_positions(joint, plan=False)
time.sleep(1)
bot.arm.go_home()
def test_vel_cmd_w(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.0, turn_speed=0.5, 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])
dt = state_after[2] - state_before[2]
dt = np.mod(dt + np.pi, 2 * np.pi) - np.pi
assert dist < 0.1
assert 0.75 < dt < 1.25
class TestRobot(unittest.TestCase):
def setUp(self):
self.robot = Robot()
def test_take_screenshot_full_screen(self):
SM_CXVIRTUALSCREEN = 78 # width of the virtual screen
SM_CYVIRTUALSCREEN = 79 # height of the virtual screen
virtual_screen_size = (
windll.user32.GetSystemMetrics(SM_CXVIRTUALSCREEN),
windll.user32.GetSystemMetrics(SM_CYVIRTUALSCREEN),
)
im = self.robot.take_screenshot() # no args: full virtual screen
self.assertEquals(im.size, virtual_screen_size)
def test_take_screenshot_bounds(self):
bounding_box = (0, 0, 100, 100)
box_size = (100, 100)
im = self.robot.take_screenshot(bounding_box)
self.assertEquals(im.size, box_size)
def test_get_display_monitors(self):
# Since the code inside of the get_display_monitors func
# is the same I'd use here to test, I'm going to just
# assume that if the first is sucesfull, the rest will be
# too.
SM_CXSCREEN = 0 # width_flag for primary
SM_CYSCREEN = 1 # height_flag for primary
primary_screen_size = (
0,
0,
windll.user32.GetSystemMetrics(SM_CXSCREEN),
windll.user32.GetSystemMetrics(SM_CYSCREEN),
)
screen_coords = self.robot.get_display_monitors()
self.assertEquals(screen_coords[0], primary_screen_size)
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Import the robot class from the pyrobot module
from pyrobot import Robot
import os
import ctypes
import multiprocessing
from ctypes import *
from ctypes.wintypes import *
# create an instance of the class
robot = Robot()
# Launch a program
robot.start_program('path')
# print(windll.kernel32)
robot.sleep(1)
robot.key_press('left_windows__(natural_board)')
# robot.press_and_release('r')
robot.key_release('left_windows__(natural_board)')
# robot.sleep(2)
# robot.type_string('string')
import os
from pyrobot import Robot
os.chdir("c:/Users/Heitor/Desktop/emacs-24.3/bin/shared/python/pontual/robo/")
robot = Robot()
robot.set_mouse_pos(40, 40)
robot.click_mouse(button="left")
robot.press_and_release("F9")
def setUp(self):
self.robot = Robot()
from pyrobot import Robot
robot = Robot()
robot.start_program('firefox.exe')
