def __init__(self, store):
GLRealtimeProgram.__init__(self, 'Motion Plan Checkpoint')
self.world = build_world(store)
self.robot = self.world.robot('tx90l')
self.robot_path = []
mp = store.get('/robot/waypoints')
self.dts = [wp[0] for wp in mp]
self.commands = [{
'robot': self.robot.getConfig()
}] + [wp[1] for wp in mp]
self.fps = 30.0
self.dt = 1 / self.fps
self.speed_scale = 1
self.end_delay = 1
self.n = 0
self.start_time = -self.end_delay
self.duration = sum(self.dts)
self.dofs = self.robot.numLinks()
self.trace_vbos = []
for i in range(1, self.dofs):
trace = self.trace_link(self.commands, i)
data = numpy.array(trace, dtype=numpy.float32)
color = cm.gist_rainbow(float(i - 1) / (self.dofs - 2))
self.trace_vbos.append((color, vbo.VBO(data, GL_STATIC_DRAW)))
def __init__(self, world, sim):
GLRealtimeProgram.__init__(self, "GLTest")
self.world = world
self.sim = sim
self.sim.enableContactFeedbackAll()
self.dt = 0.02
self.angular_velocity = 0.5
self.hand_state = 2
self.joint_limits = self.world.robot(0).getJointLimits()
self.is_forceClosure = False
self.is_equilibrium = False
self.object_id = self.world.rigidObject(0).getID()
self.num_wrench_sampling = 100
self.num_failures = 0
self.num_forceClosure = 0
self.result_success = None
self.result_gws_vol = None
self.result_gws_radius = None
self.is_lifting = False
self.time_start_lift = 0.
self.lift_score = 0.
self.is_shaking = False
self.time_start_shake = 0.
self.dir = None
self.iter_shaking = 0
self.dynamic_score = 0.0
def __init__(self,world,sim):
GLRealtimeProgram.__init__(self,"GLTest")
self.world = world
self.sim = sim
self.anim = False
#povray property
self.povray_properties = {}
#add 4 lights each with 5 meters from the robot
povray.add_multiple_lights(self.povray_properties,self.world.robot(0),5.,4,spotlight=True,area=.1)
#tell povray that all terrain will not be modified
povray.mark_terrain_transient(self.povray_properties,self.world)
#the robot will move, so we remove this line
#povray.mark_robot_transient(self.povray_properties,self.world.robot(0))
#overwrite material of floor to marble
fin=vp.Finish('ambient',0.,'diffuse',.5,'specular',.15)
nor=vp.Normal('granite',0.2,'warp {turbulence 1}','scale',.25)
povray.set_material(self.povray_properties,self.world.terrain(0),fin,nor)
#randomize robot link color
import random
for l in range(self.world.robot(0).numLinks()):
lk=self.world.robot(0).link(l)
lk.appearance().setColor(random.uniform(0.,1.),random.uniform(0.,1.),random.uniform(0.,1.))
def __init__(self):
GLRealtimeProgram.__init__(self, 'World Viewer')
self.world = WorldModel()
self.fps = 10.0
self.dt = 1 / self.fps
self.pre_drawables = []
self.post_drawables = []
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 __init__(self,world,sim):
GLRealtimeProgram.__init__(self,"GLTest")
self.world = world
self.sim = sim
def motionfunc(self, x, y, dx, dy):
return GLRealtimeProgram.motionfunc(self, x, y, dx, dy)
def mousefunc(self, button, state, x, y):
return GLRealtimeProgram.mousefunc(self, button, state, x, y)