Dokukin pull request

robs/lb1/Dokukin_DV/my_obstacle_avoidance.py

@@ -0,0 +1,78 @@
+"""my_avoid_obstacles controller."""
+
+"""Braitenberg-based obstacle-avoiding robot controller."""
+
+from controller import Robot, Compass
+
+# Get reference to the robot.
+robot = Robot()
+
+# Get robot's Compass device
+compass = robot.getCompass("compass")
+
+# Get simulation step length.
+timeStep = int(robot.getBasicTimeStep())
+
+# Constants of the Thymio II motors and distance sensors.
+maxMotorVelocity = 9.53
+distanceSensorCalibrationConstant = 360
+
+# Get left and right wheel motors.
+leftMotor = robot.getMotor("motor.left")
+rightMotor = robot.getMotor("motor.right")
+
+# Get frontal distance sensors.
+outerLeftSensor = robot.getDistanceSensor("prox.horizontal.0")
+centralLeftSensor = robot.getDistanceSensor("prox.horizontal.1")
+centralSensor = robot.getDistanceSensor("prox.horizontal.2")
+centralRightSensor = robot.getDistanceSensor("prox.horizontal.3")
+outerRightSensor = robot.getDistanceSensor("prox.horizontal.4")
+
+# Enable distance sensors.
+outerLeftSensor.enable(timeStep)
+centralLeftSensor.enable(timeStep)
+centralSensor.enable(timeStep)
+centralRightSensor.enable(timeStep)
+outerRightSensor.enable(timeStep)
+
+# Enable the Compass
+compass.enable(timeStep)
+
+# Disable motor PID control mode.
+leftMotor.setPosition(float('inf'))
+rightMotor.setPosition(float('inf'))
+
+# Set the initial velocity of the left and right wheel motors.
+leftMotor.setVelocity(maxMotorVelocity)
+rightMotor.setVelocity(maxMotorVelocity)
+
+while robot.step(timeStep) != -1:
+    # Read values from four distance sensors and calibrate.
+    outerLeftSensorValue = outerLeftSensor.getValue() / distanceSensorCalibrationConstant
+    centralLeftSensorValue = centralLeftSensor.getValue() / distanceSensorCalibrationConstant
+    centralSensorValue = centralSensor.getValue() / distanceSensorCalibrationConstant
+    centralRightSensorValue = centralRightSensor.getValue() / distanceSensorCalibrationConstant
+    outerRightSensorValue = outerRightSensor.getValue() / distanceSensorCalibrationConstant
+
+    # To read values compass
+    compassValues = compass.getValues()
+
+    # Detected obstacles
+    if outerLeftSensorValue != 0 or centralLeftSensorValue != 0:
+        leftMotor.setVelocity(maxMotorVelocity)
+        rightMotor.setVelocity(-0.5 * maxMotorVelocity) 
+    elif centralSensorValue != 0 or outerRightSensorValue != 0 or centralRightSensorValue != 0:
+        leftMotor.setVelocity(-0.5 * maxMotorVelocity)
+        rightMotor.setVelocity(maxMotorVelocity)
+
+    # Not detected obstacles
+    if outerLeftSensorValue == 0 and centralLeftSensorValue == 0 and centralSensorValue == 0 and centralRightSensorValue == 0 and outerRightSensorValue == 0:
+        if compassValues[0] > 0.001:
+            leftMotor.setVelocity(maxMotorVelocity)
+            rightMotor.setVelocity(0.9 * maxMotorVelocity)
+        elif compassValues[0] < -0.001:
+            leftMotor.setVelocity(0.9 * maxMotorVelocity)
+            rightMotor.setVelocity(maxMotorVelocity)
+        else:
+            leftMotor.setVelocity(maxMotorVelocity)
+            rightMotor.setVelocity(maxMotorVelocity)

robs/lb1/Dokukin_DV/square_path.py

@@ -0,0 +1,74 @@
+"""Sample Webots controller for the square path benchmark."""
+
+from controller import Robot
+
+# Get pointer to the robot.
+robot = Robot()
+
+# Get pointer to each wheel of our robot.
+leftWheel = robot.getMotor('left wheel')
+rightWheel = robot.getMotor('right wheel')
+
+rightWheelSensor = robot.getPositionSensor('right wheel sensor')
+rightWheelSensor.enable(1) # Refreshes the sensor every 16ms.
+
+val = 0
+print(rightWheelSensor.getValue())
+leftWheel.setVelocity(5)
+rightWheel.setVelocity(5)
+leftWheel.setPosition(5000)
+rightWheel.setPosition(5000)
+robot.step(100)
+
+
+# First set both wheels to go forward, so the robot goes straight.
+leftWheel.setVelocity(5.24)
+rightWheel.setVelocity(5.24)
+leftWheel.setPosition(5000)
+rightWheel.setPosition(5000)
+val = 20.7
+# Wait for the robot to reach a corner.
+while (rightWheelSensor.getValue() < val):
+    robot.step(1)
+print(rightWheelSensor.getValue())
+
+leftWheel.setVelocity(0)
+rightWheel.setVelocity(0)
+
+# Repeat the following 4 times (once for each side).
+for i in range(0, 3):
+    robot.step(500)
+    # Then, set the right wheel backward, so the robot will turn right.
+    leftWheel.setVelocity(5)
+    rightWheel.setVelocity(5)
+    leftWheel.setPosition(5000)
+    rightWheel.setPosition(-5000)
+    val -= 2.63 # (2.61 + 0.023 * (i%2)) # 2.59
+    # Wait until the robot has turned 90 degrees clockwise.
+    while (rightWheelSensor.getValue() > val):
+        robot.step(1)
+    print(rightWheelSensor.getValue())
+
+    leftWheel.setVelocity(0)
+    rightWheel.setVelocity(0)
+    robot.step(70)
+
+    # First set both wheels to go forward, so the robot goes straight.
+    leftWheel.setVelocity(5)
+    rightWheel.setVelocity(5)
+    leftWheel.setPosition(5000)
+    rightWheel.setPosition(5000)
+    val += 20.65
+    # Wait for the robot to reach a corner.
+    while (rightWheelSensor.getValue() < val):
+        robot.step(1)
+    print(rightWheelSensor.getValue())
+
+    leftWheel.setVelocity(0)
+    rightWheel.setVelocity(0)
+
+
+# Stop the robot when path is completed, as the robot performance
+# is only computed when the robot has stopped.
+# leftWheel.setVelocity(0)
+# rightWheel.setVelocity(0)

robs/lb2/Dokukin_Safarov/pick&place.py

@@ -0,0 +1,118 @@
+#!/usr/bin/env python
+# coding: utf-8
+
+# In[ ]:
+
+
+"""Sample Webots controller for the pick and place benchmark."""
+
+from controller import Robot
+
+# Create the Robot instance.
+robot = Robot()
+
+# Get the time step of the current world.
+timestep = int(robot.getBasicTimeStep())
+
+# Inizialize base motors.
+wheels = []
+wheels.append(robot.getMotor("wheel1"))
+wheels.append(robot.getMotor("wheel2"))
+wheels.append(robot.getMotor("wheel3"))
+wheels.append(robot.getMotor("wheel4"))
+for wheel in wheels:
+    # Activate controlling the motors setting the velocity.
+    # Otherwise by default the motor expects to be controlled in force or position,
+    # and setVelocity will set the maximum motor velocity instead of the target velocity.
+    wheel.setPosition(60)
+
+# Initialize arm motors.
+armMotors = []
+armMotors.append(robot.getMotor("arm1"))
+armMotors.append(robot.getMotor("arm2"))
+armMotors.append(robot.getMotor("arm3"))
+armMotors.append(robot.getMotor("arm4"))
+armMotors.append(robot.getMotor("arm5"))
+# Set the maximum motor velocity.
+armMotors[0].setVelocity(1)
+armMotors[1].setVelocity(0.5)
+armMotors[2].setVelocity(0.5)
+armMotors[3].setVelocity(0.3)
+
+# Initialize arm position sensors.
+# These sensors can be used to get the current joint position and monitor the joint movements.
+armPositionSensors = []
+armPositionSensors.append(robot.getPositionSensor("arm1sensor"))
+armPositionSensors.append(robot.getPositionSensor("arm2sensor"))
+armPositionSensors.append(robot.getPositionSensor("arm3sensor"))
+armPositionSensors.append(robot.getPositionSensor("arm4sensor"))
+armPositionSensors.append(robot.getPositionSensor("arm5sensor"))
+for sensor in armPositionSensors:
+    sensor.enable(timestep)
+
+# Initialize gripper motors.
+finger1 = robot.getMotor("finger1")
+finger2 = robot.getMotor("finger2")
+# Set the maximum motor velocity.
+finger1.setVelocity(0.03)
+finger2.setVelocity(0.03)
+# Read the miminum and maximum position of the gripper motors.
+fingerMinPosition = finger1.getMinPosition()
+fingerMaxPosition = finger1.getMaxPosition()
+
+
+# Move arm and open gripper.
+armMotors[1].setPosition(-0.55)
+armMotors[2].setPosition(-0.75)
+armMotors[3].setPosition(-1.6)
+finger1.setPosition(fingerMaxPosition)
+finger2.setPosition(fingerMaxPosition)
+
+# Monitor the arm joint position to detect when the motion is completed.
+while robot.step(timestep) != -1:
+    if abs(armPositionSensors[3].getValue() - (-1.2)) < 0.01:
+        # Motion completed.
+        break
+
+
+
+
+robot.step(100 * timestep)
+# Close gripper.
+finger1.setPosition(0.013)
+finger2.setPosition(0.013)
+# Wait until the gripper is closed.
+robot.step(50 * timestep)
+
+# Lift arm.
+armMotors[1].setPosition(0)
+# Wait until the arm is lifted.
+robot.step(50 * timestep)
+wheels[0].setPosition(43)
+wheels[1].setPosition(-13)
+wheels[2].setPosition(-13)
+wheels[3].setPosition(43)
+robot.step(100 * timestep)
+armMotors[0].setPosition(-2.9)
+armMotors[1].setPosition(-1.0)
+armMotors[2].setPosition(-0.3)
+armMotors[3].setPosition(0.0)
+robot.step(250 * timestep)
+wheels[0].setPosition(35)
+wheels[1].setPosition(-21)
+wheels[2].setPosition(-21)
+wheels[3].setPosition(35)
+robot.step(50 * timestep)
+
+
+
+
+
+
+
+
+# Open gripper.
+finger1.setPosition(fingerMaxPosition)
+finger2.setPosition(fingerMaxPosition)
+robot.step(50 * timestep)
+