os.path.dirname(protoPath), protoName, options, background, colorThreshold, shadowColor) # remove the object supervisor.step(timeStep) count = rootChildrenfield.getCount() importedNode.remove() supervisor.step(timeStep) if rootChildrenfield.getCount() != count - 1: sys.exit(protoName + ' was not removed sucessfully.') # Initialize the Supervisor. controller = Supervisor() timeStep = int(controller.getBasicTimeStep()) camera = controller.getCamera('camera') camera.enable(timeStep) options = get_options() if os.path.exists('.' + os.sep + 'images'): shutil.rmtree('.' + os.sep + 'images') # Get required fields rootChildrenfield = controller.getRoot().getField('children') supervisorTranslation = controller.getFromDef('SUPERVISOR').getField( 'translation') supervisorRotation = controller.getFromDef('SUPERVISOR').getField('rotation') viewpointPosition = controller.getFromDef('VIEWPOINT').getField('position') viewpointOrientation = controller.getFromDef('VIEWPOINT').getField( 'orientation') cameraNear = controller.getFromDef('CAMERA').getField('near')
rightDistanceSensor.enable(timestep) # get left and right light sensors leftLightSensor = robot.getLightSensor('lls') rightLightSensor = robot.getLightSensor('rls') leftLightSensor.enable(timestep) rightLightSensor.enable(timestep) # get left and right LED leftLED = robot.getLED('left_led') rightLED = robot.getLED('right_led') leftLED.set(1) rightLED.set(1) # enable camera and recognition camera = robot.getCamera('camera') camera.enable(timestep) camera.recognitionEnable(timestep) # get handler to motors and set target position to infinity leftMotor = robot.getMotor('left wheel motor') rightMotor = robot.getMotor('right wheel motor') leftMotor.setPosition(float('inf')) rightMotor.setPosition(float('inf')) leftMotor.setVelocity(0) rightMotor.setVelocity(0) global wallCounter# global variable to use as ID for wall instances # get timestep def getTimestep():
class AutobinEnv(gym.Env): metadata = {'render.modes': ['human']} def __init__(self, time_step): # time step in ms self.time_step = time_step # supevisor node self.robot = Supervisor() # self node to get velocity and position self.bin = self.robot.getSelf() # cameras self.cameras = [] camerasNames = ['camera1', 'camera2'] for name in camerasNames: self.cameras.append(self.robot.getCamera(name)) self.cameras[-1].enable(self.time_step) self.img_size = (self.cameras[0].getWidth(), self.cameras[0].getHeight()) # wheels self.wheels = [] wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4'] for name in wheelsNames: self.wheels.append(self.robot.getMotor(name)) self.wheels[-1].setPosition(float('inf')) self.wheels[-1].setVelocity(0.0) # ball self.ball = self.robot.getFromDef('Ball') def step(self, action): self.wheels[0].setVelocity(action[0]) self.wheels[1].setVelocity(action[1]) self.wheels[2].setVelocity(action[0]) self.wheels[3].setVelocity(action[1]) # step the world if self.robot.step(self.time_step) == -1: print('The world finishes!') return None imgs = [ Image.frombytes('RGB', self.img_size, cam.getImage()) for cam in self.cameras ] vel = self.bin.getVelocity() pos = self.bin.getPosition() done = False reward = 0 contact = self.ball.getNumberOfContactPoints() if contact > 0: vel_len = math.sqrt(vel[0] * vel[0] + vel[1] * vel[1]) if self.bin.getNumberOfContactPoints() > 0: reward = 20 - vel_len print( f'Autobin successfully catches the ball! Reward is {reward:.3f}' ) else: contact_pos = self.ball.getContactPoint(0) dx = contact_pos[0] - pos[0] dy = contact_pos[1] - pos[1] dist = math.sqrt(dx * dx + dy * dy) reward = -vel_len - dist print(f'Autobin misses ball! Reward is {reward:.3f}') self.reset() return imgs, reward, done, {'velocity': vel, 'position': pos} def reset(self): pos = self.ball.getField('translation') pos.setSFVec3f([0, 5, 0]) max_vel = 2 theta = random.random() * 2 * math.pi vel_x = max_vel * math.cos(theta) vel_y = max_vel * math.sin(theta) self.ball.setVelocity([vel_x, vel_y, 0, 0, 0, 0]) def render(self, mode='human'): pass
class DarwinController: def __init__(self, namespace='', node=True): self.time = 0 self.clock_msg = Clock() self.namespace = namespace self.supervisor = Supervisor() self.motor_names = [ "ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL", "ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL", "LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR", "AnkleL", "FootR", "FootL", "Neck", "Head" ] self.walkready = [0] * 20 self.names_webots_to_bitbots = { "ShoulderR": "RShoulderPitch", "ShoulderL": "LShoulderPitch", "ArmUpperR": "RShoulderRoll", "ArmUpperL": "LShoulderRoll", "ArmLowerR": "RElbow", "ArmLowerL": "LElbow", "PelvYR": "RHipYaw", "PelvYL": "LHipYaw", "PelvR": "RHipRoll", "PelvL": "LHipRoll", "LegUpperR": "RHipPitch", "LegUpperL": "LHipPitch", "LegLowerR": "RKnee", "LegLowerL": "LKnee", "AnkleR": "RAnklePitch", "AnkleL": "LAnklePitch", "FootR": "RAnkleRoll", "FootL": "LAnkleRoll", "Neck": "HeadPan", "Head": "HeadTilt" } self.names_bitbots_to_webots = { "RShoulderPitch": "ShoulderR", "LShoulderPitch": "ShoulderL", "RShoulderRoll": "ArmUpperR", "LShoulderRoll": "ArmUpperL", "RElbow": "ArmLowerR", "LElbow": "ArmLowerL", "RHipYaw": "PelvYR", "LHipYaw": "PelvYL", "RHipRoll": "PelvR", "LHipRoll": "PelvL", "RHipPitch": "LegUpperR", "LHipPitch": "LegUpperL", "RKnee": "LegLowerR", "LKnee": "LegLowerL", "RAnklePitch": "AnkleR", "LAnklePitch": "AnkleL", "RAnkleRoll": "FootR", "LAnkleRoll": "FootL", "HeadPan": "Neck", "HeadTilt": "Head" } self.motors = [] self.sensors = [] self.timestep = int(self.supervisor.getBasicTimeStep()) self.timestep = 10 for motor_name in self.motor_names: self.motors.append(self.supervisor.getMotor(motor_name)) self.motors[-1].enableTorqueFeedback(self.timestep) self.sensors.append( self.supervisor.getPositionSensor(motor_name + "S")) self.sensors[-1].enable(self.timestep) self.accel = self.supervisor.getAccelerometer("Accelerometer") self.accel.enable(self.timestep) self.gyro = self.supervisor.getGyro("Gyro") self.gyro.enable(self.timestep) self.camera = self.supervisor.getCamera("Camera") self.camera.enable(self.timestep) if node: rospy.init_node("webots_darwin_ros_interface", anonymous=True, argv=['clock:=/' + self.namespace + '/clock']) self.pub_js = rospy.Publisher(self.namespace + "/joint_states", JointState, queue_size=1) self.pub_imu = rospy.Publisher(self.namespace + "/imu/data", Imu, queue_size=1) self.pub_cam = rospy.Publisher(self.namespace + "/image_raw", Image, queue_size=1) self.clock_publisher = rospy.Publisher(self.namespace + "/clock", Clock, queue_size=1) rospy.Subscriber(self.namespace + "/DynamixelController/command", JointCommand, self.command_cb) self.world_info = self.supervisor.getFromDef("world_info") self.hinge_joint = self.supervisor.getFromDef("barrier_hinge") self.robot_node = self.supervisor.getFromDef("Darwin") self.translation_field = self.robot_node.getField("translation") self.rotation_field = self.robot_node.getField("rotation") def step_sim(self): self.supervisor.step(self.timestep) def step(self): self.step_sim() self.time += self.timestep / 1000 self.publish_imu() self.publish_joint_states() self.clock_msg.clock = rospy.Time.from_seconds(self.time) self.clock_publisher.publish(self.clock_msg) def command_cb(self, command: JointCommand): for i, name in enumerate(command.joint_names): try: motor_index = self.motor_names.index( self.names_bitbots_to_webots[name]) self.motors[motor_index].setPosition(command.positions[i]) except ValueError: print( f"invalid motor specified ({self.names_bitbots_to_webots[name]})" ) self.publish_joint_states() self.publish_imu() self.publish_camera() def publish_joint_states(self): js = JointState() js.name = [] js.header.stamp = rospy.get_rostime() js.position = [] js.effort = [] for i in range(len(self.sensors)): js.name.append(self.names_webots_to_bitbots[self.motor_names[i]]) value = self.sensors[i].getValue() js.position.append(value) js.effort.append(self.motors[i].getTorqueFeedback()) self.pub_js.publish(js) def publish_imu(self): msg = Imu() msg.header.stamp = rospy.get_rostime() msg.header.frame_id = "imu_frame" accel_vels = self.accel.getValues() msg.linear_acceleration.x = ((accel_vels[0] - 512.0) / 512.0) * 3 * G msg.linear_acceleration.y = ((accel_vels[1] - 512.0) / 512.0) * 3 * G msg.linear_acceleration.z = ((accel_vels[2] - 512.0) / 512.0) * 3 * G gyro_vels = self.gyro.getValues() msg.angular_velocity.x = ((gyro_vels[0] - 512.0) / 512.0) * 1600 * ( math.pi / 180) # is 400 deg/s the real value msg.angular_velocity.y = ( (gyro_vels[1] - 512.0) / 512.0) * 1600 * (math.pi / 180) msg.angular_velocity.z = ( (gyro_vels[2] - 512.0) / 512.0) * 1600 * (math.pi / 180) self.pub_imu.publish(msg) def publish_camera(self): img_msg = Image() img_msg.header.stamp = rospy.get_rostime() img_msg.height = self.camera.getHeight() img_msg.width = self.camera.getWidth() img_msg.encoding = "bgra8" img_msg.step = 4 * self.camera.getWidth() img = self.camera.getImage() img_msg.data = img self.pub_cam.publish(img_msg) def set_gravity(self, active): if active: self.world_info.getField("gravity").setSFVec3f([0.0, -9.81, 0.0]) else: self.world_info.getField("gravity").setSFVec3f([0.0, 0.0, 0.0]) def reset_robot_pose(self, pos, quat): rpy = tf.transformations.euler_from_quaternion(quat) self.set_robot_pose_rpy(pos, rpy) self.robot_node.resetPhysics() def reset_robot_pose_rpy(self, pos, rpy): self.set_robot_pose_rpy(pos, rpy) self.robot_node.resetPhysics() def reset(self): self.supervisor.simulationReset() def node(self): s = self.supervisor.getSelected() if s is not None: print(f"id: {s.getId()}, type: {s.getType()}, def: {s.getDef()}") def set_robot_pose_rpy(self, pos, rpy): self.translation_field.setSFVec3f(pos_ros_to_webots(pos)) self.rotation_field.setSFRotation(rpy_to_axis(*rpy)) def get_robot_pose_rpy(self): pos = self.translation_field.getSFVec3f() rot = self.rotation_field.getSFRotation() return pos_webots_to_ros(pos), axis_to_rpy(*rot)
timestep = 100 conn = socket.socket(socket.AF_INET, socket.SOCK_STREAM) motors = [ supervisor.getMotor("shoulder_pan_joint"), supervisor.getMotor("shoulder_lift_joint"), supervisor.getMotor("elbow_joint"), supervisor.getMotor("wrist_1_joint"), supervisor.getMotor("wrist_2_joint"), supervisor.getMotor("wrist_3_joint") ] #motor_sensors = [robot.getPositionSensor("shoulder_pan_joint_sensor"), robot.getPositionSensor("shoulder_lift_joint_sensor"), robot.getPositionSensor("elbow_joint_sensor"), #robot.getPositionSensor("wrist_1_joint_sensor"), robot.getPositionSensor("wrist_2_joint_sensor"), robot.getPositionSensor("wrist_3_joint_sensor")] camera = supervisor.getCamera('cameraRGB') depth_camera = supervisor.getRangeFinder('cameraDepth') ur_node = supervisor.getFromDef('UR5') camera_transform_node = ur_node.getField('children').getMFNode(0) camera_node = camera_transform_node.getField('children').getMFNode(1) #depth_camera.enable(35) phone_part_objects = [ supervisor.getFromDef('Bottom_Cover'), supervisor.getFromDef('Bottom_Cover_2'), supervisor.getFromDef('White_Cover'), supervisor.getFromDef('White_Cover_2'), supervisor.getFromDef('Black_Cover'), supervisor.getFromDef('Black_Cover_2'), supervisor.getFromDef('Blue_Cover'), supervisor.getFromDef('Blue_Cover_2'),