def reset(self):
print('reset')
del self.sim
world.robot(0).setConfig(self.q_init)
world.rigidObject(0).setTransform(*self.object_init)
self.sim = Simulator(world)
self.controller = self.sim.controller(0)
self.set_goal(self.store_goal)
def __init__(self, robot, sim_world):
self.robot = robot
self.sim_world = sim_world
self.suspend = False
self.sim = Simulator(sim_world)
self.sim.setGravity([0, 0, 0])
self.controller = self.sim.controller(0)
self.controller.setRate(parameters.CONTROLLER_DT)
def run_cartesian(world, paths):
sim_world = world
sim_robot = world.robot(0)
vis.add("world", world)
planning_world = world.copy()
planning_robot = planning_world.robot(0)
sim = Simulator(sim_world)
robot_controller = RobotInterfaceCompleter(
KinematicSimControlInterface(sim_robot))
#TODO: Uncomment this if you'd like to test in the physics simulation
#robot_controller = RobotInterfaceCompleter(SimPositionControlInterface(sim.controller(0),sim))
if not robot_controller.initialize():
raise RuntimeError("Can't connect to robot controller")
ee_link = 'EndEffector_Link'
ee_local_pos = (0.15, 0, 0)
ee_local_axis = (1, 0, 0)
lifth = 0.05
drawing_controller = DrawingController(robot_controller, planning_robot,
planning_robot.getConfig(), paths,
ee_link, ee_local_pos,
ee_local_axis, (0, 0, 1), lifth)
controller_vis = RobotInterfacetoVis(robot_controller)
#note: this "storage" argument is only necessary for jupyter to keep these around and not destroy them once main() returns
def callback(robot_controller=robot_controller,
drawing_controller=drawing_controller,
storage=[sim_world, planning_world, sim, controller_vis]):
start_clock = time.time()
dt = 1.0 / robot_controller.controlRate()
#advance the controller
robot_controller.startStep()
if robot_controller.status() == 'ok':
drawing_controller.advance(dt)
vis.addText("Status", drawing_controller.state, (10, 20))
robot_controller.endStep()
#update the visualization
sim_robot.setConfig(
robot_controller.configToKlampt(robot_controller.sensedPosition()))
Tee = sim_robot.link(ee_link).getTransform()
vis.add(
"pen axis",
Trajectory([0, 1], [
se3.apply(Tee, ee_local_pos),
se3.apply(Tee,
vectorops.madd(ee_local_pos, ee_local_axis, lifth))
]),
color=(0, 1, 0, 1))
controller_vis.update()
cur_clock = time.time()
duration = cur_clock - start_clock
time.sleep(max(0, dt - duration))
vis.loop(callback=callback)
class PhysicsSim:
def __init__(self, robot, sim_world):
self.robot = robot
self.sim_world = sim_world
self.suspend = False
self.sim = Simulator(sim_world)
self.sim.setGravity([0, 0, 0])
self.controller = self.sim.controller(0)
self.controller.setRate(parameters.CONTROLLER_DT)
def suspend(self):
self.suspend = True
def run(self):
while not self.suspend:
self.sim.updateWorld()
self.sim.simulate(parameters.CONTROLLER_DT)
class MyGLViewer(GLRealtimeProgram):
def __init__(self, world, sim):
GLRealtimeProgram.__init__(self, "GL test")
self.world = world
self.sim = sim
self.controller = self.sim.controller(0)
print('gains ', self.controller.getPIDGains())
self.goal = None
self.store_goal = None
# add function to reset the world
self.q_init = world.robot(0).getConfig()
self.object_init = world.rigidObject(0).getTransform()
def display(self):
self.sim.updateWorld()
self.world.drawGL()
def idle(self):
if self.goal is not None:
self.controller.setMilestone(self.goal)
print(self.controller.getPIDGains())
self.goal = None
self.sim.simulate(self.dt)
if self.sim.getTime() % 3.0 <= 1e-6:
self.reset()
def set_goal(self, goal):
self.goal = goal
self.store_goal = goal
def reset(self):
print('reset')
del self.sim
world.robot(0).setConfig(self.q_init)
world.rigidObject(0).setTransform(*self.object_init)
self.sim = Simulator(world)
self.controller = self.sim.controller(0)
self.set_goal(self.store_goal)
def run_simulation(world):
value = resource.get('start.config',default=world.robot(0).getConfig(),world=world)
world.robot(0).setConfig(value)
sim_world = world
sim_robot = world.robot(0)
vis.add("world",world)
planning_world = world.copy()
planning_robot = planning_world.robot(0)
sim = Simulator(sim_world)
# robot_controller = RobotInterfaceCompleter(KinematicSimControlInterface(sim_robot))
#TODO: Uncomment this when you are ready for testing in the physics simulation
robot_controller = RobotInterfaceCompleter(SimPositionControlInterface(sim.controller(0),sim))
if not robot_controller.initialize():
raise RuntimeError("Can't connect to robot controller")
ee_link = 'EndEffector_Link'
ee_local_pos = (0.15,0,0)
ee_local_axis = (1,0,0)
lifth = 0.05
drawing_controller = CircleController(robot_controller,planning_robot,ee_link,
ee_local_pos,ee_local_axis,
radius=0.05,period=5.0)
controller_vis = RobotInterfacetoVis(robot_controller)
trace = Trajectory()
#note: this "storage" argument is only necessary for jupyter to keep these around and not destroy them once main() returns
def callback(robot_controller=robot_controller,drawing_controller=drawing_controller,trace=trace,
storage=[sim_world,planning_world,sim,controller_vis]):
start_clock = time.time()
dt = 1.0/robot_controller.controlRate()
#advance the controller
robot_controller.startStep()
drawing_controller.advance(dt)
robot_controller.endStep()
#update the visualization
sim_robot.setConfig(robot_controller.configToKlampt(robot_controller.sensedPosition()))
Tee=sim_robot.link(ee_link).getTransform()
wp = se3.apply(Tee,ee_local_pos)
if len(trace.milestones) == 0 or vectorops.distance(wp,trace.milestones[-1]) > 0.01:
trace.milestones.append(wp)
trace.times.append(0)
if len(trace.milestones)==2:
vis.add("trace",trace,color=(0,1,1,1),pointSize=0)
if len(trace.milestones) > 200:
trace.milestones = trace.milestones[-100:]
trace.times = trace.times[-100:]
if len(trace.milestones)>2:
if _backend=='IPython':
vis.remove("trace")
vis.add("trace",trace,color=(0,1,1,1),pointSize=0)
else:
vis.dirty("trace")
controller_vis.update()
cur_clock = time.time()
duration = cur_clock - start_clock
time.sleep(max(0,dt-duration))
vis.loop(callback=callback)
robot.setJointLimits(qmin, qmax)
config = robot.getConfig()
config[:] = q0[:len(config)].tolist()
robot.setConfig(config)
end_link = robot.link(6) # for redundancy
end_link_pos = end_link.getWorldPosition([0, 0, 0])
object_pos = vo.add(end_link_pos, [0, 0.0, 0.18])
print('object at ', object_pos)
box = create.primitives.box(0.1,
0.1,
0.1,
object_pos,
world=world,
name='box',
mass=3)
print(type(box))
print(robot.getConfig())
path = np.zeros((plan.shape[0], len(config)))
path[:] = plan[:, :len(config)]
path = path.tolist()
# this allows visualization, but not real simulation
# visualize(world, robot, path.tolist())
sim = Simulator(world)
sim.setSetting("adaptiveTimeStepping", "0")
viewer = MyGLViewer(world, sim)
viewer.set_goal(path[-1])
viewer.run()
global solved_trajectory, trajectory_is_transfer, executing_plan, execute_start_time
if solved_trajectory is None:
return
executing_plan = True
if PHYSICS_SIMULATION:
execute_start_time = 0
solved_trajectory = path_to_trajectory(solved_trajectory,
timing='robot',
smoothing=None)
solved_trajectory.times = [10 * t for t in solved_trajectory.times]
else:
execute_start_time = time.time()
vis.addAction(executePlan, "Execute plan", 'e')
sim = Simulator(world)
sim_dt = 0.02
sensor = sim.controller(0).sensor("realsense")
sensor2 = sim.controller(0).sensor("realsense")
# vis.add("sensor", sensor)
was_grasping = False
swab_time = 0
swab_init_height = 0
swab_height = 0
def loop_callback():
global was_grasping, Tobject_grasp, solved_trajectory, trajectory_is_transfer, executing_plan, \
execute_start_time, qstart, next_robot_to_move, swab_time, swab_init_height, swab_height
global solved_trajectory, trajectory_is_transfer, executing_plan, execute_start_time
if solved_trajectory is None:
return
executing_plan = True
if PHYSICS_SIMULATION:
execute_start_time = 0
solved_trajectory = path_to_trajectory(solved_trajectory,
timing='robot',
smoothing=None)
solved_trajectory.times = [10 * t for t in solved_trajectory.times]
else:
execute_start_time = time.time()
vis.addAction(executePlan, "Execute plan", 'e')
sim = Simulator(world)
sim_dt = 0.02
was_grasping = False
def loop_callback():
global was_grasping, Tobject_grasp, solved_trajectory, trajectory_is_transfer, executing_plan, execute_start_time, qstart, next_item_to_pick
if not executing_plan:
return
if PHYSICS_SIMULATION:
execute_start_time += sim_dt
t = execute_start_time
else:
t = time.time() - execute_start_time
vis.addText("time", "Time %.3f" % (t), position=(10, 10))
if PROBLEM == '3c':
qstart = solved_trajectory.eval(t)
raise RuntimeError("Couldn't read the world file")
shelf = make_shelf(w,*shelf_dims)
shelf.geometry().translate((shelf_offset_x,shelf_offset_y,shelf_height))
obj = w.makeRigidObject("point_cloud_object") #Making Box
obj.geometry().loadFile(KLAMPT_Demo+"/data/objects/apc/genuine_joe_stir_sticks.pcd")
#set up a "reasonable" inertial parameter estimate for a 200g object
m = obj.getMass()
m.estimate(obj.geometry(),0.200)
obj.setMass(m)
#we'll move the box slightly forward so the robot can reach it
obj.setTransform(so3.identity(),[shelf_offset_x-0.05,shelf_offset_y-0.3,shelf_height+0.01])
vis.add("world",w)
vis.add("ghost",w.robot(0).getConfig(),color=(0,1,0,0.5))
vis.edit("ghost")
from klampt import Simulator
sim = Simulator(w)
def setup():
vis.show()
def callback():
sim.controller(0).setPIDCommand(vis.getItemConfig("ghost"),[0]*w.robot(0).numLinks())
sim.simulate(0.01)
sim.updateWorld()
vis.loop(setup=setup,callback=callback)