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 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()