def did_collide():
while True:
if robot.is_colliding():
t = pypot_time.time()
robot.last_collision = t
robot.recent_collisions.append(t)
time.sleep(0.02)
def setup(self):
if self.duration < numpy.finfo(float).eps:
self.motor.goal_position = self.goal
self.stop()
else:
self.trajs = MinimumJerkTrajectory(self.motor.present_position, self.goal, self.duration).get_generator()
self.t0 = time.time()
def elapsed_time(self):
return time.time() - self.t0
def setup(self):
self.trajs = MinimumJerkTrajectory(self.motor.present_position, self.goal, self.duration).get_generator()
self.t0 = time.time()
def _prim_setup(self):
logger.info("Primitive %s setup.", self)
self.robot._primitive_manager.add(self)
self.setup()
self.t0 = time.time()
def _wrapped_update(self):
logger.debug('LoopPrimitive %s updated.', self)
self._recent_updates.append(time.time())
self.update()
def elapsed_time(self):
""" Elapsed time (in seconds) since the primitive runs. """
return time.time() - self.t0
def elapsed_time(self):
return time.time() - self.t0
def _prim_setup(self):
logger.info("Primitive %s setup.", self)
self.robot._primitive_manager.add(self)
self.setup()
self.t0 = time.time()
def _wrapped_update(self):
logger.debug('LoopPrimitive %s updated.', self)
self._recent_updates.append(time.time())
self.update()
def elapsed_time(self):
""" Elapsed time (in seconds) since the primitive runs. """
return time.time() - self.t0
def setup(self):
self.trajs = MinimumJerkTrajectory(self.motor.present_position,
self.goal,
self.duration).get_generator()
self.t0 = time.time()
def update(self):
self.lfoot = self.vrepio.call_remote_api('simxGetObjectPosition', self.obj_h[
'foot_left_visual'], self.dummies_h['CoM'], streaming=True)
self.rfoot = self.vrepio.call_remote_api('simxGetObjectPosition', self.obj_h[
'foot_right_visual'], self.dummies_h['CoM'], streaming=True)
self.com_abs = self.vrepio.call_remote_api(
'simxGetObjectPosition', self.dummies_h['CoM'], - 1, streaming=True)
self.com_vel = self.vrepio.call_remote_api(
'simxGetObjectVelocity', self.dummies_h['CoM'], streaming=True)
self.lfoot = np.array(
[self.lfoot[0], self.lfoot[1], self.lfoot[2]])
self.rfoot = np.array(
[self.rfoot[0], self.rfoot[1], self.rfoot[2]])
self.com = np.array(
[self.com_abs[0], self.com_abs[1], self.com_abs[2]])
lfoot_pos_com = np.array(self.lfoot)
rfoot_pos_com = np.array(self.rfoot)
dt = time.time() - self.t0
self.t0 = time.time()
if dt <= 0.0:
self.lfoot_vel_com = np.array([0, 0, 0])
self.rfoot_vel_com = np.array([0, 0, 0])
else:
self.lfoot_vel_com = (
np.array(lfoot_pos_com) - np.array(self.lfoot_pos_com)) / dt
self.rfoot_vel_com = (
np.array(rfoot_pos_com) - np.array(self.rfoot_pos_com)) / dt
self.lfoot_pos_com = lfoot_pos_com
self.rfoot_pos_com = rfoot_pos_com
if self.com_abs[2] < 0.3:
print 'FALL:', self.state
self.fall = True
self.robot.reset_simulation()
print 'resetting'
self.nb_reset += 1
if self.nb_reset == 5:
print self.quasix.Compute(self.goal_vertx)
print
print self.quasiy.Compute(self.goal_verty)
print self.quasix.ComputePolicy()
print
print self.quasiy.ComputePolicy()
self.nb_reset = 0
else:
if self.robot.CollisionLFoot.colliding and not self.robot.CollisionRFoot.colliding:
# Only LFoot on ground
self.state = 'right'
elif not self.robot.CollisionLFoot.colliding and self.robot.CollisionRFoot.colliding:
# only RFoot on ground
self.state = 'left'
elif self.robot.CollisionLFoot.colliding and self.robot.CollisionRFoot.colliding:
# Both Feet on ground. Take the decision here.
self.state = 'both'
else:
# gnin?
print 'FLY'
self.state = 'fly'
def Init(self):
self.robot._primitive_manager._filter = np.sum
# self.quasix = quasix
# self.quasiy = quasiy
# self.Xpos = Xpos
# self.Ypos = Ypos
# self.Xvel = Xvel
# self.Yvel = Yvel
# self.Ux = Ux
# self.Uy = Uy
# self.X = X
# self.Y = Y
self.set_goal(0, 0.05, 0, 0)
self.fall = False
self.nb_reset = 0
self.transitions_log = open(
'onlineqm_trans_' + str(self.port) + '.dat', 'a+')
self.transitions_logd = open(
'onlineqm_transd_' + str(self.port) + '.dat', 'a+')
self.state = 'right'
self.goal_leg = 0
self.duration = 0.25
self.up = 0.1
self.vrepio = self.robot._controllers[0].io
self.obj_h = {}
objects = self.vrepio.call_remote_api('simxGetObjectGroupData',
vrep.sim_object_shape_type, 0)
self.obj_h = dict(zip(objects[3], objects[0]))
self.joint_h = {}
joints = self.vrepio.call_remote_api('simxGetObjectGroupData',
vrep.sim_object_joint_type, 0)
self.joint_h = dict(zip(joints[3], joints[0]))
self.dummies_h = {}
dummies = self.vrepio.call_remote_api('simxGetObjectGroupData',
vrep.sim_object_dummy_type, 0)
self.dummies_h = dict(zip(dummies[3], dummies[0]))
self.lfoot = self.vrepio.call_remote_api('simxGetObjectPosition', self.obj_h[
'foot_left_visual'], self.dummies_h['CoM'], streaming=True)
self.rfoot = self.vrepio.call_remote_api('simxGetObjectPosition', self.obj_h[
'foot_right_visual'], self.dummies_h['CoM'], streaming=True)
self.com_abs = self.vrepio.call_remote_api(
'simxGetObjectPosition', self.dummies_h['CoM'], - 1, streaming=True)
self.com_vel = self.vrepio.call_remote_api(
'simxGetObjectVelocity', self.dummies_h['CoM'], streaming=True)
self.lfoot = np.array(
[self.lfoot[0], self.lfoot[1], self.lfoot[2]])
self.rfoot = np.array(
[self.rfoot[0], self.rfoot[1], self.rfoot[2]])
self.com = np.array(
[self.com_abs[0], self.com_abs[1], self.com_abs[2]])
self.lfoot_pos_com = np.array(self.lfoot)
self.rfoot_pos_com = np.array(self.rfoot)
self.lfoot_vel_com = np.array([0, 0, 0])
self.rfoot_vel_com = np.array([0, 0, 0])
self.robot.attach_primitive(
min_jerk.MJLegsUp3D(self.robot, 'left', self.up), 'lleg3d_up')
self.robot.attach_primitive(
min_jerk.MJLegsUp3D(self.robot, 'right', self.up), 'rleg3d_up')
self.robot.attach_primitive(
min_jerk.MJLegsDown3D(self.robot, 'left', self.up), 'lleg3d_down')
self.robot.attach_primitive(
min_jerk.MJLegsDown3D(self.robot, 'right', self.up), 'rleg3d_down')
self.t0 = time.time()
# time.sleep(0.1)
self.update()
self.current_state = (self.rfoot_pos_com[0], -self.rfoot_pos_com[
1], self.rfoot_vel_com[0], -self.rfoot_vel_com[1])
self.current_cmd = None
def setup(self):
self.t0 = time.time()
