Beispiel #1
0
                    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)
Beispiel #5
0
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'),