1. Analysis and description of controller code
First of all, please visit:http://www.running-robot.net/download/206.html , open after downloading walk.wbt Documents. Click toolbar wizard new robot controller, select Python language, complete the creation, and open it in the text editor. Copy the attachment code, overwrite the default code, and save.
Set robot controller as new controller and save the world.
In walk.py to control the robot, we need to introduce several Python libraries.
1. from controller import Robot
2. import os
3. import sys
4.
5. libraryPath = os.path.join(os.environ.get("WEBOTS_HOME"), 'projects', 'robots', 'robotis', 'darwin-op', 'libraries',
6. 'python37')
7. libraryPath = libraryPath.replace('/', os.sep)
8. sys.path.append(libraryPath)
9. from managers import RobotisOp2GaitManager, RobotisOp2MotionManager
The robot library which controls the robot environment includes the control of various robot components. RobotisOp2GaitManager and RobotisOp2MotionManager libraries are gait control database and action group database, respectively, to realize the robot's walking control and pre programmed action execution. In order to operate normally in external IDEs such as Pysharm, path information needs to be added before importing these two libraries.
Note: this code is applicable to Python version 3.7. Other versions of Python may have compatibility problems.
Define the walk class to control the robot. The initial function is as follows.
1. class Walk():
2. def __init__(self):
3. self.robot = Robot() # Initialize robot to control robot
4. self.mTimeStep = int(self.robot.getBasicTimeStep()) # Get the current simulation time of each simulation step mTimeStep
5. self.HeadLed = self.robot.getLED('HeadLed') # Get head LED light
6. self.EyeLed = self.robot.getLED('EyeLed') # Get eye LED light
7. self.HeadLed.set(0xff0000) # Turn on the head led and set a color
8. self.EyeLed.set(0xa0a0ff) # Turn on the eye led and set a color
9. self.mAccelerometer = self.robot.getAccelerometer('Accelerometer') # Acquire acceleration sensor
10. self.mAccelerometer.enable(self.mTimeSt ep) # Activate the sensor and update the value with mtimestep as the cycle
11. self.fup = 0
12. self.fdown = 0 # Two variables are defined to determine whether the robot falls or not
13.
14. self.mGyro = self.robot.getGyro('Gyro') # Get gyroscope
15. self.mGyro.enable(self.mTimeStep) # Activate gyroscope,and update the value with mTimeStep as the cycle
16.
17. self.positionSensors = [] # Initialize joint angle sensor
18. self.positionSensorNames = ('ShoulderR', 'ShoulderL', 'ArmUpperR', 'ArmUpperL',
19. 'ArmLowerR', 'ArmLowerL', 'PelvYR', 'PelvYL',
20. 'PelvR', 'PelvL', 'LegUpperR', 'LegUpperL',
21. 'LegLowerR', 'LegLowerL', 'AnkleR', 'AnkleL',
22. 'FootR', 'FootL', 'Neck', 'Head') # Initialize each sensor name
23.
24. # Acquire and activate each sensor, and update the value with mTimeStep as the cycle
25. for i in range(0, len(self.positionSensorNames)):
26. self.positionSensors.append(self.robot.getPositionSensor(self.positionSensorNames[i] + 'S'))
27. self.positionSensors[i].enable(self.mTimeStep)
28.
29. self.mKeyboard = self.robot.getKeyboard() # Initialize keyboard read in class
30. self.mKeyboard.enable(self.mTimeStep) # Read from keyboard with mTimeStep as cycle
31.
32. self.mMotionManager = RobotisOp2MotionManager(self.robot) # Initialize robot action group controller
33. self.mGaitManager = RobotisOp2GaitManager(self.robot, " Head ") # Initialize robot gait controller
Define the myStep() function in walk to complete a simulation step.
1. def myStep(self):
2. ret = self.robot.step(self.mTimeStep)
3. if ret == -1:
4. exit(0)
Define the wait() function in Walk, input as the number of milliseconds to wait for the robot for a period of time.
1. def wait(self, ms):
2. startTime = self.robot.getTime()
3. s = ms / 1000.0
4. while (s + startTime >= self.robot.getTime()):
5. self.myStep()
run() function to control the robot's motion through keyboard input:
1. def run(self):
2. print("-------Walk example of ROBOTIS OP2-------")
3. print("This example illustrates Gait Manager")
4. print("Press the space bar to start/stop walking")
5. print("Use the arrow keys to move the robot while walking")
6. self.myStep() # Simulate a step to refresh the sensor reading
7.
8. self.mMotionManager.playPage(9) # Perform action group 9, initialize standing position, and prepare to walk
9. self.wait(200) # wait 200ms
10.
11. self.isWalking = False # Initially, the robot did not enter the walking state
12.
13. while True:
14. self.checkIfFallen() # Determine if you fall
15. self.mGaitManager.setXAmplitude(0.0) # Forward 0
16. self.mGaitManager.setAAmplitude(0.0) # Turn 0
17. key = 0 # Initial keyboard read in defaults 0
18. key = self.mKeyboard.getKey() # Read input from keyboard
19. if key == 32: # If a space is read, the walking state will be changed
20. if (self.isWalking): # Stop the robot if it is currently walking
21. self.mGaitManager.stop()
22. self.isWalking = False
23. self.wait(200)
24. else: # If the robot is currently stopped, start walking
25. self.mGaitManager.start()
26. self.isWalking = True
27. self.wait(200)
28. elif key == 315: # If reading‘↑’,move forward
29. self.mGaitManager.setXAmplitude(1.0)
30. elif key == 317: # If reading‘↓’,move backward
31. self.mGaitManager.setXAmplitude(-1.0)
32. elif key == 316: # If reading‘←’,turn left
33. self.mGaitManager.setAAmplitude(-0.5)
34. elif key == 314: # If reading‘→’,turn right
35. self.mGaitManager.setAAmplitude(0.5)
36. self.mGaitManager.step(self.mTimeStep) # Gait generator generates a step action
37. self.myStep() # Simulate one step
checkIfFallen function is used to detect whether the robot falls to the ground and complete the action of rising from the ground:
1. def checkIfFallen(self):
2. acc_tolerance = 60.0
3. acc_step = 100 # Counter upper limit
4. acc = self.mAccelerometer.getValues() # Obtain the corresponding value of three axes through acceleration sensor
5. if acc[1] < 512.0 - acc_tolerance : # The value of y-axis will be smaller when face down
6. self.fup += 1 # Counter add 1
7. else :
8. self.fup = 0 # Counter clear
9. if acc[1] > 512.0 + acc_tolerance : # The value of the y-axis will increase when you fall back to the ground
10. self.fdown += 1 # Counter add 1
11. else :
12. self.fdown = 0 # Counter clear
13.
14. if self.fup > acc_step : # The counter exceeds the upper limit, that is the fall time exceeds acc_ Step simulation steps
15. self.mMotionManager.playPage(10) # Perform a face down, floor up motion
16. self.mMotionManager.playPage(9) # Get ready to walk
17. self.fup = 0 # Counter clear
18. elif self.fdown > acc_step :
19. self.mMotionManager.playPage(11) # Perform a back down and up motion
20. self.mMotionManager.playPage(9) # Get ready to walk
21. self.fdown = 0 # Counter clear
The acceleration value of robot Y-axis is obtained by acceleration sensor. When the value is greater than or less than a certain value for a period of time, it is judged that the robot falls with its back down / face down, and then the corresponding up action is performed.
The main function is defined as follows:
1. if __name__ == '__main__':
2. walk = Walk() # Initializing the walk
3. walk.run() # Run controller
2. Modify the code to control the robot's motion
Modify the run() function as follows to make the robot walk automatically without keyboard input.
1. def run(self):
2. self.myStep() # Simulate a step to refresh the sensor reading
3.
4. self.mMotionManager.playPage(9) # Perform action group 9, initialize standing position, and prepare to walk
5. self.wait(200) # Wait 200ms
6.
7. self.isWalking = False # Initially, the robot did not enter the walking state
8. while True:
9. self.checkIfFallen() # Determine if fall or not
10. self.mGaitManager.start() # Gait generator enters walking state
11. self.mGaitManager.setXAmplitude(1.0) # Set robot forward
12. self.mGaitManager.step(self.mTimeStep) # Gait generator generates a step action
13. self.myStep() # Simulate one step
Save the code text after modification, as shown below.
Then rerun the simulation, you can see that the robot starts to move forward automatically.
In a while loop self.mGaitManager.setXAmplitude (1.0) statement enables the robot to move forward continuously. If you want the robot to stop at an appropriate position, you can modify the condition of the while loop.
3 list of common functions
Function name | Purpose | Note |
mGaitManager.start () | Start | |
mGaitManager.stop () | Stop | |
mGaitManager.step (t) | Generate gait planning for a period of time | t in milliseconds |
mGaitManager.setXAmplitude (double X) | Forward/backward | X affects the forward length of a step, and can take any value between - 1 and 1 |
mGaitManager.setYAmplitude (double Y) | Move left / right | Y affects the length of the foot in the side direction, it can take any value between - 1 and 1 |
mGaitManager.setAAmplitude (double A) | Turn left / right | A affects the angle of gait and allows the robot to rotate during walking. It can take any value between 0 and 1 |
mKeyboard.getKey() | Get keyboard input | The value obtained is the ASCII code corresponding to the input charact-er |
wait (int t) | Wait | t in milliseconds |
mMotionManager.playPage() | Perform corresponding actions | playPage is a series of encapsulated robot action files |
4 Partial action list of playPage
Action group number | Name | Description | Initial attitude |
9 | walk ready | Ready to wal-k | Stand |
10 | f up | Get up | Face down |
11 | b up | Get up | Back down |
更 GFor more actions, you can query the related documents of robots - robot op2 in the webots user guide.
5 Modify the configuration file to adjust the robot's stance and gait
From webotsprojects obots obotisdarwin opcontrollerswalk config.ini , copy one copy to the path where the controller code is located. After that, it passes the code initialization section self.mGaitManager = RobotisOp2GaitManager( self.robot " config.ini ") statement to load the configuration file. You can use Notepad to open it, as shown below:
You can modify the values in the configuration file to adjust the standing posture and gait of the robot. The specific explanation of each parameter can be found in the appendix of robots - robot op2 in the webots user guide.