def set_position(self, x, y, z):
#return
object = self.robot.getFromDef(self.name)
positionField = object.getField("translation")
Field.setSFVec3f(positionField, [y, z, x])
for _ in range(5):
self.robot.step(
self.timestep) # if not nan values in first iteration
def resetRobotPosition(self):
#tempZip = zip(self.motor_names, self.INITIAL_MOTOR_POS)
#jointPositions = dict(tempZip)
#random_init_pos = dict()
#for k in self.DEFAULT_MOTOR_POS.keys():
# self.INITIAL_MOTOR_POS[k] = max(min(self.DEFAULT_MOTOR_POS[k] + np.random.uniform(low=-0.05, high = 0.05), 1), -1)
#print(k,self.INITIAL_MOTOR_POS[k])
self.setJointPositions(self.INITIAL_MOTOR_POS)
for i in range(100):
self.step(self.timeStep)
Field.setSFVec3f(self.trans_field, self.INITIAL_TRANS)
Field.setSFRotation(self.rot_field, self.INITIAL_ROT)
#print(jointPositions)
#wb_supervisor_simulation_reset_physics
Supervisor.simulationResetPhysics(self)
#print('-------------------------------------')
#print('>>>>>>Robot pos has been reset<<<<<<')
#print('-------------------------------------')
return True