├── .gitignore ├── mathematics ├── cpg │ ├── image │ │ └── result.png │ ├── webots_results │ │ ├── pace.mp4 │ │ ├── trot.mp4 │ │ ├── walk.mp4 │ │ └── gallop.mp4 │ └── code.py ├── hopf │ ├── image │ │ └── result.png │ └── code.py ├── walk_trot_transition │ ├── image │ │ └── result.png │ └── code.py └── inverse_kinematic │ ├── results │ ├── TrajectoryPlanning.png │ ├── thetaList │ └── targetPoints │ ├── plot.py │ ├── TrajectoryPlanner.py │ ├── FK_2joints.py │ ├── trajectory.py │ └── taskspace.py ├── hardware_test ├── LabRobot │ ├── video │ │ └── walk_trot.mp4 │ ├── StableGait │ │ ├── StableGait.ino │ │ ├── test │ │ │ ├── test.ino │ │ │ ├── Actuator.h │ │ │ └── Actuator.cpp │ │ ├── QuadrupedRobot.h │ │ └── QuadrupedRobot.cpp │ ├── motorTest │ │ └── motorTest.ino │ ├── DogRobot │ │ ├── DogRobot.ino │ │ ├── QuadrupedRobot.h │ │ └── QuadrupedRobot.cpp │ └── motorsTest │ │ └── motorsTest.ino ├── singleLeg │ ├── Readme.txt │ ├── singleLeg.ino │ ├── Robot3R.cpp │ └── Robot3R.h ├── motorTest │ └── motorTest.ino ├── motorsTest │ └── motorsTest.ino └── Quadruped │ └── Quadruped.ino ├── webots_sim ├── controllers │ ├── cpg_controller │ │ ├── cpg_controller │ │ ├── build │ │ │ └── release │ │ │ │ ├── cpg_controller │ │ │ │ ├── cpg_controller.o │ │ │ │ └── cpg_controller.d │ │ ├── cpg_controller.cpp │ │ └── Makefile │ ├── cpg_hopf_controller │ │ ├── cpg_hopf_controller │ │ ├── build │ │ │ └── release │ │ │ │ ├── cpg_hopf_controller │ │ │ │ ├── cpg_hopf_controller.o │ │ │ │ └── cpg_hopf_controller.d │ │ ├── Makefile │ │ └── cpg_hopf_controller.cpp │ ├── single_leg_knimatics │ │ └── single_leg_knimatics.py │ ├── init_testing │ │ └── init_testing.py │ └── quadruped_controller │ │ └── quadruped_controller.py ├── protos │ ├── .MyWorkbanch.cache │ ├── .DotGo.cache │ ├── MyWorkbanch.proto │ └── DotGo.proto └── worlds │ ├── wild_field.wbt │ ├── .wild_field.wbproj │ ├── .mini_dog.wbproj │ ├── .quadruped_robot.wbproj │ ├── mini_dog.wbt │ └── quadruped_robot.wbt └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | .idea 3 | 4 | .vscode -------------------------------------------------------------------------------- /mathematics/cpg/image/result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/mathematics/cpg/image/result.png -------------------------------------------------------------------------------- /mathematics/hopf/image/result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/mathematics/hopf/image/result.png -------------------------------------------------------------------------------- /mathematics/cpg/webots_results/pace.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/mathematics/cpg/webots_results/pace.mp4 -------------------------------------------------------------------------------- /mathematics/cpg/webots_results/trot.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/mathematics/cpg/webots_results/trot.mp4 -------------------------------------------------------------------------------- /mathematics/cpg/webots_results/walk.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/mathematics/cpg/webots_results/walk.mp4 -------------------------------------------------------------------------------- /hardware_test/LabRobot/video/walk_trot.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/hardware_test/LabRobot/video/walk_trot.mp4 -------------------------------------------------------------------------------- /mathematics/cpg/webots_results/gallop.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/mathematics/cpg/webots_results/gallop.mp4 -------------------------------------------------------------------------------- /mathematics/walk_trot_transition/image/result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/mathematics/walk_trot_transition/image/result.png -------------------------------------------------------------------------------- /webots_sim/controllers/cpg_controller/cpg_controller: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/webots_sim/controllers/cpg_controller/cpg_controller -------------------------------------------------------------------------------- /mathematics/inverse_kinematic/results/TrajectoryPlanning.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/mathematics/inverse_kinematic/results/TrajectoryPlanning.png -------------------------------------------------------------------------------- /webots_sim/controllers/cpg_hopf_controller/cpg_hopf_controller: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/webots_sim/controllers/cpg_hopf_controller/cpg_hopf_controller -------------------------------------------------------------------------------- /webots_sim/controllers/cpg_controller/build/release/cpg_controller: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/webots_sim/controllers/cpg_controller/build/release/cpg_controller -------------------------------------------------------------------------------- /webots_sim/controllers/cpg_controller/build/release/cpg_controller.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/webots_sim/controllers/cpg_controller/build/release/cpg_controller.o -------------------------------------------------------------------------------- /webots_sim/controllers/cpg_hopf_controller/build/release/cpg_hopf_controller: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/webots_sim/controllers/cpg_hopf_controller/build/release/cpg_hopf_controller -------------------------------------------------------------------------------- /webots_sim/controllers/cpg_hopf_controller/build/release/cpg_hopf_controller.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/col-in-coding/robot-modeling/HEAD/webots_sim/controllers/cpg_hopf_controller/build/release/cpg_hopf_controller.o -------------------------------------------------------------------------------- /mathematics/inverse_kinematic/plot.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | 4 | x = np.arange(0, 10, 0.2) 5 | y = np.sin(x) 6 | fig, ax = plt.subplots() 7 | ax.plot(x, y) 8 | plt.show() 9 | 10 | plt.show() 11 | -------------------------------------------------------------------------------- /webots_sim/protos/.MyWorkbanch.cache: -------------------------------------------------------------------------------- 1 | Webots Proto Cache File version 1.4 2 | protoFileHash: a1464cbcc4ded3bbf603f88f56de3ef7 3 | containsDevices: 0 4 | baseType: Solid 5 | slotType: 6 | parameters: translation,scale 7 | tags: 8 | license: 9 | licenseUrl: 10 | documentationUrl: 11 | info: 12 | -------------------------------------------------------------------------------- /webots_sim/protos/.DotGo.cache: -------------------------------------------------------------------------------- 1 | Webots Proto Cache File version 1.4 2 | protoFileHash: a35424b4416c0b5771d70a1084bad017 3 | containsDevices: 1 4 | baseType: Robot 5 | slotType: 6 | parameters: translation,rotation,controller 7 | tags: 8 | license: 9 | licenseUrl: 10 | documentationUrl: 11 | info: 12 | -------------------------------------------------------------------------------- /hardware_test/singleLeg/Readme.txt: -------------------------------------------------------------------------------- 1 | Title: Single Leg Test for the Robot Dog 2 | 3 | Description: The single leg is equal to a 3R robot arm with angle range limits 4 | Parameters: 5 | motor: w_max = 7.4798 6 | theta0: Hip joint1, 30 ~ 90 deg 7 | theta1: Hip joint2, 45 ~ 135 deg 8 | theta2: knee joint, 0 ~ 180 deg 9 | -------------------------------------------------------------------------------- /mathematics/inverse_kinematic/TrajectoryPlanner.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import pandas as pd 4 | 5 | class TrajectoryPlanner: 6 | 7 | def __init__(self): 8 | self.link = np.array([]) 9 | # angle limits 10 | self.angleRange1 = np.pi / 2 11 | self.angleRange2 = np.pi 12 | -------------------------------------------------------------------------------- /webots_sim/controllers/cpg_controller/build/release/cpg_controller.d: -------------------------------------------------------------------------------- 1 | build/release/cpg_controller.o: cpg_controller.cpp \ 2 | /home/colin/webots/include/controller/cpp/webots/Robot.hpp \ 3 | /home/colin/webots/include/controller/cpp/webots/Motor.hpp \ 4 | /home/colin/webots/include/controller/cpp/webots/Device.hpp \ 5 | /home/colin/webots/include/controller/cpp/webots/../../c/webots/types.h 6 | -------------------------------------------------------------------------------- /webots_sim/controllers/cpg_hopf_controller/build/release/cpg_hopf_controller.d: -------------------------------------------------------------------------------- 1 | build/release/cpg_hopf_controller.o: cpg_hopf_controller.cpp \ 2 | /home/colin/webots/include/controller/cpp/webots/Robot.hpp \ 3 | /home/colin/webots/include/controller/cpp/webots/Motor.hpp \ 4 | /home/colin/webots/include/controller/cpp/webots/Device.hpp \ 5 | /home/colin/webots/include/controller/cpp/webots/../../c/webots/types.h 6 | -------------------------------------------------------------------------------- /hardware_test/singleLeg/singleLeg.ino: -------------------------------------------------------------------------------- 1 | #include "Robot3R.h" 2 | 3 | Robot3R robot = Robot3R(5, 6, 7); 4 | 5 | void setup() { 6 | pinMode(2, INPUT_PULLUP); 7 | pinMode(3, INPUT_PULLUP); 8 | pinMode(4, INPUT_PULLUP); 9 | pinMode(5, INPUT_PULLUP); 10 | pinMode(6, INPUT_PULLUP); 11 | pinMode(7, INPUT_PULLUP); 12 | Serial.begin(9600); 13 | robot.start(); 14 | } 15 | 16 | void loop() { 17 | robot.run_period(); 18 | } 19 | -------------------------------------------------------------------------------- /webots_sim/worlds/wild_field.wbt: -------------------------------------------------------------------------------- 1 | #VRML_SIM R2019b utf8 2 | WorldInfo { 3 | basicTimeStep 16 4 | } 5 | Viewpoint { 6 | orientation -0.003362672587841199 -0.9993500733294183 -0.03589043562779957 3.1240263453539217 7 | position 0.14079037660940208 0.3412590110971202 -3.098557895534884 8 | } 9 | TexturedBackground { 10 | } 11 | TexturedBackgroundLight { 12 | } 13 | CircleArena { 14 | } 15 | MyWorkbanch { 16 | } 17 | DotGo { 18 | translation 0 0.42 0 19 | } 20 | -------------------------------------------------------------------------------- /mathematics/inverse_kinematic/FK_2joints.py: -------------------------------------------------------------------------------- 1 | ### 2 | # Two joints Leg Forward kinematic 3 | # 4 | # Inputs: 5 | # l1: link1 length 6 | # l2: link2 length 7 | # (a, b): end effect coordinate 8 | # 9 | # Output: 10 | # T: Homogeneous Transformation Matrix of End Effect 11 | # 12 | # Formulas: 13 | # a = c01 * x12 - s01 * y12 + x01 14 | # b = s01 * x12 + c01 * y12 + y01 15 | # l1 ** 2 = x01 ** 2 + y01 ** 2 16 | # l2 ** 2 = x12 ** 2 + y12 ** 2 17 | # theta01 = arctan(y01 / x01) 18 | # theta12 = arctan(y12 / x12) 19 | # theta02 = theta01 + theta12 20 | ### 21 | 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # robot-modeling 2 | Quadruped Robot Design 3 | 4 | ## Single Leg Design 5 | For the common gaits like walk and trot, the trajectories of legs are the same way with different phases, so the study can be simplifed. A 3R robot model is build for the study. 6 | 7 | ### Mathematics 8 | Python 9 | 10 | Base on the gait, the simple trajectory route can be designed in taskspace of the end effect.\ 11 | Here is the manually design for simple:\ 12 | 13 | 14 | ### Simulation 15 | Webots 16 | 17 | ### Hardwares 18 | Arduino Uno, Quadruped Robot 19 | 20 | -------------------------------------------------------------------------------- /hardware_test/LabRobot/StableGait/StableGait.ino: -------------------------------------------------------------------------------- 1 | #include "QuadrupedRobot.h" 2 | 3 | QuadrupedRobot Robot; 4 | 5 | void setup() { 6 | Serial.begin(9600); 7 | Serial.setTimeout(10); 8 | while (!Serial); 9 | // wait for ready 10 | while (Serial.available() && Serial.read()); // empty buffer 11 | delay(1000); 12 | 13 | Serial.print( 14 | "Please select from menu: \n" 15 | " 1. switch on\n" 16 | " 2. stand\n" 17 | " 3. walk\n" 18 | " 4. turn left\n" 19 | " 5. turn right\n" 20 | " 0. switch off\n" 21 | " 998. adjust angles\n" 22 | ); 23 | } 24 | 25 | void loop() { 26 | Robot.interrupt(); 27 | Robot.parse_code_run(); 28 | } 29 | -------------------------------------------------------------------------------- /hardware_test/LabRobot/motorTest/motorTest.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // our servo # counter 4 | uint8_t servonum = 9; 5 | Servo s; 6 | 7 | void setup() { 8 | Serial.begin(9600); 9 | Serial.println("Lab robot servo test!"); 10 | s.attach(servonum, 500, 2500); 11 | } 12 | 13 | void writeServo(uint8_t angle) { 14 | double pulse; 15 | pulse = (0.5 + angle/90.0) * 1000; 16 | Serial.print("pulse:"); 17 | Serial.println(pulse); 18 | s.writeMicroseconds(pulse); 19 | } 20 | 21 | String inString = ""; 22 | void loop() { 23 | if (Serial.available() > 0) { 24 | if (Serial.peek() != '\n') { 25 | inString += (char)Serial.read(); 26 | } else { 27 | Serial.read(); 28 | Serial.print("Instruction received: "); 29 | Serial.println(inString); 30 | writeServo(inString.toInt()); 31 | inString = ""; 32 | } 33 | } 34 | } 35 | -------------------------------------------------------------------------------- /mathematics/hopf/code.py: -------------------------------------------------------------------------------- 1 | ### 2 | # Hopf Oscillator Model 3 | # 4 | # Params: 5 | # alpha: control the rate of convergence 6 | # u: control the amplitude of the output signals, A = sqrt(u) 7 | # w: (omega) control the frequency 8 | ### 9 | 10 | import matplotlib.pyplot as plt 11 | import numpy as np 12 | 13 | x0 = np.random.random() 14 | y0 = np.random.random() 15 | 16 | fig, ax = plt.subplots() 17 | 18 | alpha = 100 19 | u = 1 20 | w = 2 * np.pi 21 | dt = 0.01 22 | iteration = 100 23 | 24 | x = x0 25 | y = y0 26 | xt = [x0] 27 | yt = [y0] 28 | for t in np.arange(0, 5, dt): 29 | for i in np.arange(iteration): 30 | r_square = x ** 2 + y ** 2 31 | x_dot = alpha * (u - r_square) * x - w * y 32 | y_dot = alpha * (u - r_square) * y + w * x 33 | x += x_dot * dt / iteration 34 | y += y_dot * dt / iteration 35 | xt.append(x) 36 | yt.append(y) 37 | ax.plot(xt, yt) 38 | 39 | plt.show() 40 | -------------------------------------------------------------------------------- /webots_sim/worlds/.wild_field.wbproj: -------------------------------------------------------------------------------- 1 | Webots Project File version R2019b 2 | perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff000000010000018c000003c8fc0200000001fb0000001400540065007800740045006400690074006f00720100000019000003c80000008900ffffff000000030000078000000077fc0100000001fb0000000e0043006f006e0073006f006c00650000000000000007800000006900ffffff000005f2000003c800000001000000020000000100000008fc00000000 3 | simulationViewPerspectives: 000000ff000000010000000200000100000005620100000002010000000100 4 | sceneTreePerspectives: 000000ff000000010000000200000284000000fa0100000002010000000200 5 | maximizedDockId: -1 6 | centralWidgetVisible: 1 7 | selectionDisabled: 0 8 | viewpointLocked: 0 9 | orthographicViewHeight: 1 10 | textFiles: 1 "controllers/single_leg_knimatics/single_leg_knimatics.py" "controllers/init_testing/init_testing.py" 11 | -------------------------------------------------------------------------------- /webots_sim/controllers/single_leg_knimatics/single_leg_knimatics.py: -------------------------------------------------------------------------------- 1 | """single_leg_knimatics controller.""" 2 | 3 | # You may need to import some classes of the controller module. Ex: 4 | # from controller import Robot, Motor, DistanceSensor 5 | from controller import Robot 6 | 7 | # create the Robot instance. 8 | robot = Robot() 9 | 10 | # get the time step of the current world. 11 | timestep = int(robot.getBasicTimeStep()) 12 | 13 | motor = robot.getMotor('servo_knee4') 14 | motor.setVelocity(10) 15 | motor.setPosition(1) 16 | 17 | # Main loop: 18 | # - perform simulation steps until Webots is stopping the controller 19 | while robot.step(timestep) != -1: 20 | # Read the sensors: 21 | # Enter here functions to read sensor data, like: 22 | # val = ds.getValue() 23 | 24 | # Process sensor data here. 25 | 26 | # Enter here functions to send actuator commands, like: 27 | # motor.setPosition(10.0) 28 | pass 29 | 30 | # Enter here exit cleanup code. 31 | -------------------------------------------------------------------------------- /hardware_test/LabRobot/StableGait/test/test.ino: -------------------------------------------------------------------------------- 1 | #include "Actuator.h" 2 | 3 | Actuator Act; 4 | 5 | void setup() { 6 | // put your setup code here, to run once: 7 | Serial.begin(9600); 8 | Serial.setTimeout(10); 9 | while (!Serial); 10 | // wait for ready 11 | while (Serial.available() && Serial.read()); // empty buffer 12 | delay(1000); 13 | 14 | Act.init(); 15 | 16 | Serial.print( 17 | "Please select from menu: \n" 18 | " 1. adjust\n" 19 | " 2. stand\n" 20 | ); 21 | } 22 | 23 | void loop() { 24 | // put your main code here, to run repeatedly: 25 | String serial_in_str {""}; 26 | while(Serial.available() > 0) { 27 | char inChar = Serial.read(); 28 | serial_in_str += (char)inChar; 29 | // wait for input buffer read 30 | delay(10); 31 | } 32 | if(serial_in_str != "") 33 | { 34 | Serial.print("Command received: "); 35 | Serial.println(serial_in_str); 36 | int command_code = serial_in_str.toInt(); 37 | 38 | Act.adjust(); 39 | 40 | 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /webots_sim/worlds/.mini_dog.wbproj: -------------------------------------------------------------------------------- 1 | Webots Project File version R2019b 2 | perspectives: 000000ff00000000fd000000030000000000000069000003c8fc0100000002fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000000690000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003cb000002cbfc0200000001fb0000001400540065007800740045006400690074006f00720100000019000002cb0000008900ffffff0000000300000780000000fbfc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007800000006900ffffff000003b3000002cb00000001000000020000000100000008fc00000000 3 | simulationViewPerspectives: 000000ff000000010000000200000000000003b10100000002010000000100 4 | sceneTreePerspectives: 000000ff000000010000000200000284000000fa0100000002010000000200 5 | maximizedDockId: -1 6 | centralWidgetVisible: 1 7 | selectionDisabled: 0 8 | viewpointLocked: 0 9 | orthographicViewHeight: 1 10 | textFiles: 0 "controllers/cpg_hopf_controller/cpg_hopf_controller.cpp" "controllers/cpg_controller/cpg_controller.cpp" 11 | globalOptionalRendering: JointAxes::ConnectorAxes::CoordinateSystem 12 | -------------------------------------------------------------------------------- /hardware_test/LabRobot/StableGait/test/Actuator.h: -------------------------------------------------------------------------------- 1 | #ifndef ACTUATOR_H 2 | #define ACTUATOR_H 3 | 4 | #include 5 | #include "Actuator.h" 6 | #include "Adafruit_PWMServoDriver.h" 7 | 8 | 9 | struct Servo 10 | { 11 | uint8_t id; 12 | int8_t dir; 13 | float init_ang; // in degrees 14 | float angle; // current angle in degrees 15 | Servo(uint8_t _id, int8_t _dir, float _init_ang) { 16 | id = _id; 17 | dir = _dir; 18 | init_ang = _init_ang; 19 | angle = _init_ang; 20 | } 21 | }; 22 | typedef Servo* ServoPtr; 23 | 24 | 25 | struct Leg 26 | { 27 | ServoPtr sRoll; 28 | ServoPtr sPitch; 29 | ServoPtr sKnee; 30 | }; 31 | typedef Leg* LegPtr; 32 | 33 | 34 | class Actuator 35 | { 36 | private: 37 | const uint8_t dof{12}; 38 | const uint8_t freq{50}; // Hz 39 | const uint16_t resolution{4096}; // 12 bits 40 | Adafruit_PWMServoDriver *pwm; 41 | LegPtr legFL; 42 | LegPtr legFR; 43 | LegPtr legRR; 44 | LegPtr legRL; 45 | 46 | void set_servo_pulse(ServoPtr servo, double pulse); 47 | void write_servo_ang(ServoPtr servo, float ang); 48 | 49 | public: 50 | Actuator(/* args */); 51 | ~Actuator(); 52 | void init(); 53 | void adjust(); 54 | 55 | }; 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /mathematics/inverse_kinematic/trajectory.py: -------------------------------------------------------------------------------- 1 | import pandas as pd 2 | import numpy as np 3 | 4 | targetPoints = pd.read_csv('./results/targetPoints').to_numpy() 5 | link1 = 11 6 | link2 = 9 7 | 8 | initAng = [] 9 | 10 | theta = [] 11 | 12 | 13 | def rad_to_ang(rad): 14 | return int(rad * 180 / np.pi) 15 | 16 | 17 | # calculate the position of knee joint and R of end effect 18 | for tx, ty in targetPoints: 19 | d = np.sqrt(tx ** 2 + ty ** 2) 20 | v = (link1 ** 2 + d ** 2 - link2 ** 2) / (2 * link1 * d) 21 | 22 | if v > 1: 23 | print('theta error') 24 | hTheta1 = 0 25 | else: 26 | hTheta1 = np.arccos((link1 ** 2 + d ** 2 - link2 ** 2) / (2 * link1 * d)) 27 | 28 | theta3 = np.arccos((link2 ** 2 + d ** 2 - link1 ** 2) / (2 * link2 * d)) 29 | hTheta2 = np.arctan(tx / ty) 30 | # preset theta0 31 | theta0 = 0 32 | theta1 = rad_to_ang(hTheta1 + hTheta2) 33 | theta2 = rad_to_ang(hTheta1 + theta3) 34 | 35 | theta.append([theta0, theta1, theta2]) 36 | 37 | theta = np.array(theta) 38 | theta1 = theta[..., 1] 39 | 40 | output = pd.DataFrame({ 41 | 'theta0': theta[..., 0], 42 | 'theta1': theta[..., 1], 43 | 'theta2': theta[..., 2] 44 | }) 45 | output.to_csv('./results/thetaList', index=False) 46 | -------------------------------------------------------------------------------- /mathematics/inverse_kinematic/results/thetaList: -------------------------------------------------------------------------------- 1 | theta0,theta1,theta2 2 | 0,16,36 3 | 0,17,36 4 | 0,17,36 5 | 0,18,36 6 | 0,19,36 7 | 0,19,35 8 | 0,20,35 9 | 0,20,35 10 | 0,21,34 11 | 0,22,34 12 | 0,22,33 13 | 0,22,32 14 | 0,23,31 15 | 0,23,31 16 | 0,23,30 17 | 0,24,29 18 | 0,24,27 19 | 0,24,26 20 | 0,24,25 21 | 0,24,23 22 | 0,24,21 23 | 0,24,19 24 | 0,23,17 25 | 0,23,14 26 | 0,22,9 27 | 0,22,9 28 | 0,27,22 29 | 0,30,30 30 | 0,33,36 31 | 0,35,41 32 | 0,37,46 33 | 0,38,50 34 | 0,39,54 35 | 0,40,57 36 | 0,41,60 37 | 0,42,63 38 | 0,42,66 39 | 0,43,68 40 | 0,43,70 41 | 0,43,73 42 | 0,43,74 43 | 0,43,76 44 | 0,42,78 45 | 0,42,79 46 | 0,41,80 47 | 0,41,81 48 | 0,40,82 49 | 0,39,82 50 | 0,38,83 51 | 0,37,83 52 | 0,35,83 53 | 0,34,83 54 | 0,33,82 55 | 0,31,82 56 | 0,30,81 57 | 0,29,80 58 | 0,27,79 59 | 0,25,78 60 | 0,24,76 61 | 0,22,74 62 | 0,21,73 63 | 0,19,70 64 | 0,17,68 65 | 0,15,66 66 | 0,14,63 67 | 0,12,60 68 | 0,10,57 69 | 0,8,54 70 | 0,6,50 71 | 0,4,46 72 | 0,1,41 73 | 0,0,36 74 | 0,-3,30 75 | 0,-7,22 76 | 0,-13,9 77 | 0,-13,9 78 | 0,-10,14 79 | 0,-8,17 80 | 0,-6,19 81 | 0,-5,21 82 | 0,-3,23 83 | 0,-2,25 84 | 0,0,26 85 | 0,0,27 86 | 0,1,29 87 | 0,3,30 88 | 0,4,31 89 | 0,5,31 90 | 0,6,32 91 | 0,7,33 92 | 0,8,34 93 | 0,9,34 94 | 0,10,35 95 | 0,11,35 96 | 0,12,35 97 | 0,13,36 98 | 0,14,36 99 | 0,14,36 100 | 0,15,36 101 | 0,16,36 102 | -------------------------------------------------------------------------------- /webots_sim/worlds/.quadruped_robot.wbproj: -------------------------------------------------------------------------------- 1 | Webots Project File version R2019b 2 | perspectives: 000000ff00000000fd000000030000000000000069000003c8fc0100000002fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900fffffffb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000000690000006900ffffff00000001000001c400000325fc0200000001fb0000001400540065007800740045006400690074006f00720100000019000003250000008900ffffff0000000300000780000000a1fc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007800000006900ffffff000005ba0000032500000001000000020000000100000008fc00000000 3 | simulationViewPerspectives: 000000ff000000010000000200000214000003a40100000002010000000100 4 | sceneTreePerspectives: 000000ff0000000100000002000001ee000000fa0100000002010000000200 5 | minimizedPerspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000003320000034ffc0200000001fb0000001400540065007800740045006400690074006f007201000000190000034f0000008900ffffff000000030000078000000077fc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007800000006900ffffff0000044c0000034f00000001000000020000000100000008fc00000000 6 | maximizedDockId: -1 7 | centralWidgetVisible: 1 8 | selectionDisabled: 0 9 | viewpointLocked: 0 10 | orthographicViewHeight: 1 11 | textFiles: 0 "controllers/quadruped_controller/quadruped_controller.py" 12 | globalOptionalRendering: JointAxes::CoordinateSystem 13 | -------------------------------------------------------------------------------- /webots_sim/protos/MyWorkbanch.proto: -------------------------------------------------------------------------------- 1 | PROTO MyWorkbanch [ 2 | field SFVec3f translation 0 0.218272 0 3 | field SFVec3f scale 1 1 1 4 | ] 5 | { 6 | Solid { 7 | translation IS translation 8 | scale IS scale 9 | children [ 10 | DEF WORKBANCH_GROUP Group { 11 | children [ 12 | DEF TOP Transform { 13 | translation 0 0.16 0 14 | children [ 15 | Shape { 16 | appearance PBRAppearance { 17 | baseColor 0.333333 0.341176 0.32549 18 | } 19 | geometry Box { 20 | size 0.1 0.02 0.4 21 | } 22 | } 23 | ] 24 | } 25 | DEF BOTTOM Transform { 26 | translation 0 -0.2 0 27 | children [ 28 | Shape { 29 | appearance PBRAppearance { 30 | baseColor 0.333333 0.341176 0.32549 31 | } 32 | geometry Box { 33 | size 0.5 0.05 0.5 34 | } 35 | } 36 | ] 37 | } 38 | DEF CYLINDER Shape { 39 | appearance PBRAppearance { 40 | baseColor 0.333333 0.341176 0.32549 41 | metalness 0 42 | } 43 | geometry Cylinder { 44 | height 0.34 45 | radius 0.06 46 | } 47 | } 48 | ] 49 | } 50 | ] 51 | boundingObject USE WORKBANCH_GROUP 52 | physics Physics { 53 | density 2000 54 | } 55 | } 56 | } -------------------------------------------------------------------------------- /hardware_test/motorTest/motorTest.ino: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "Adafruit_PWMServoDriver.h" 4 | 5 | // called this way, it uses the default address 0x40 6 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40); 7 | 8 | // our servo # counter 9 | uint8_t servonum = 0; 10 | 11 | void setup() { 12 | Serial.begin(9600); 13 | Serial.println("16 channel Servo test!"); 14 | pwm.begin(); 15 | pwm.setPWMFreq(50); // Analog servos run at ~60 Hz updates 16 | } 17 | 18 | // you can use this function if you'd like to set the pulse length in seconds 19 | // e.g. setServoPulse(0, 0.001) is a ~1 millisecond pulse width. its not precise! 20 | void setServoPulse(uint8_t n, double pulse) { 21 | double pulselength; 22 | 23 | pulselength = 1000000; // 1,000,000 us per second 24 | pulselength /= 50; // 50 Hz 25 | Serial.print(pulselength); Serial.println(" us per period"); 26 | pulselength /= 4096; // 12 bits of resolution 27 | Serial.print(pulselength); Serial.println(" us per bit"); 28 | pulse *= 1000; 29 | pulse /= pulselength; 30 | Serial.println(pulse); 31 | pwm.setPWM(n, 0, pulse); 32 | } 33 | 34 | void writeServo(uint8_t n,uint8_t angle){ 35 | double pulse; 36 | pulse=0.5+angle/90.0; 37 | setServoPulse(n,pulse); 38 | } 39 | String inString = ""; 40 | 41 | void loop() { 42 | if (Serial.available() > 0) { 43 | if (Serial.peek() != '\n') { 44 | inString += (char)Serial.read(); 45 | } else { 46 | Serial.read(); 47 | Serial.print("Instruction received: "); 48 | Serial.println(inString); 49 | writeServo(servonum,inString.toInt()); 50 | inString = ""; 51 | } 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /hardware_test/LabRobot/DogRobot/DogRobot.ino: -------------------------------------------------------------------------------- 1 | #include "QuadrupedRobot.h" 2 | 3 | QuadrupedRobot Robot; 4 | 5 | void setup() { 6 | Serial.begin(9600); 7 | Serial.setTimeout(10); 8 | while (!Serial); 9 | // wait for ready 10 | while (Serial.available() && Serial.read()); // empty buffer 11 | delay(1000); 12 | 13 | PTL("Please select from menu: "); 14 | PTL(" 1. switch on"); 15 | PTL(" 2. stand"); 16 | PTL(" 3. walk"); 17 | PTL(" 4. trot"); 18 | PTL(" 0. switch off"); 19 | PTL(" 998. adjust angles"); 20 | } 21 | 22 | int inp = ""; 23 | int code = 999; 24 | void loop() { 25 | if (Serial.available() > 0) { 26 | if (Serial.peek() != '\n') { 27 | inp = Serial.parseInt(); 28 | } else { 29 | Serial.read(); 30 | Serial.print("Instruction received: "); 31 | Serial.println(inp); 32 | 33 | code = inp; 34 | Robot.lock = false; // unlock the robot 35 | inp = ""; 36 | } 37 | } 38 | 39 | switch(code) { 40 | case 1: 41 | Robot.switch_on(); 42 | // lock the robot, unless the periodic motion code 43 | Robot.lock = true; 44 | break; 45 | case 2: 46 | Robot.bot_stand(); 47 | Robot.lock = true; 48 | break; 49 | case 3: 50 | Robot.bot_walk(); 51 | // Robot.lock = true; // for test 52 | break; 53 | case 4: 54 | Robot.bot_trot(); 55 | break; 56 | case 0: 57 | Robot.switch_off(); 58 | Robot.lock = true; 59 | break; 60 | case 998: 61 | Robot.adjust(); 62 | Robot.lock = true; 63 | case 999: 64 | // waiting for input ... 65 | break; 66 | default: 67 | PTL("wrong code"); 68 | code = 999; 69 | } 70 | 71 | } 72 | -------------------------------------------------------------------------------- /hardware_test/singleLeg/Robot3R.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * Author: Haoxiang Li (Colin) 3 | * 3R Robot 4 | * Source Code 5 | *******************************************************************/ 6 | 7 | #include "Arduino.h" 8 | #include "Robot3R.h" 9 | #include 10 | 11 | // servos 12 | Servo hip0_servo; 13 | Servo hip1_servo; 14 | Servo knee_servo; 15 | 16 | Robot3R::Robot3R(byte hip0_p, byte hip1_p, byte knee_p) { 17 | hip0_pin = hip0_p; 18 | hip1_pin = hip1_p; 19 | knee_pin = knee_p; 20 | } 21 | 22 | void Robot3R::start() { 23 | Serial.println("start robot in 3 second:"); 24 | hip0_servo.attach(hip0_pin); 25 | hip1_servo.attach(hip1_pin); 26 | knee_servo.attach(knee_pin); 27 | servo_set_angs(current_theta); 28 | delay(3000); 29 | } 30 | 31 | /** 32 | * Convert Angle Radius to PWM us 33 | */ 34 | int Robot3R::convert_rad_to_pulse_us(double rad) { 35 | int us = SERVO_US_MIN + (SERVO_US_MAX - SERVO_US_MIN) * rad / 3.14; 36 | return us; 37 | } 38 | 39 | /** 40 | * Convert Angle Degree to PWM us 41 | */ 42 | int Robot3R::convert_ang_to_pulse_us(int16_t ang) { 43 | int us = SERVO_US_MIN + static_cast(SERVO_US_MAX - SERVO_US_MIN) * ang / 180; 44 | return us; 45 | } 46 | 47 | void Robot3R::servo_set_angs(int16_t angs[]) { 48 | int16_t ang0 = INIT_ANG[0] + SERVO_DIR[0] * angs[0]; 49 | int16_t ang1 = INIT_ANG[1] + SERVO_DIR[1] * angs[1]; 50 | int16_t ang2 = INIT_ANG[2] + SERVO_DIR[2] * angs[2]; 51 | // Serial.println(ang0); 52 | // Serial.println(ang1); 53 | // Serial.println(ang2); 54 | hip0_servo.writeMicroseconds(convert_ang_to_pulse_us(ang0)); 55 | hip1_servo.writeMicroseconds(convert_ang_to_pulse_us(ang1)); 56 | knee_servo.writeMicroseconds(convert_ang_to_pulse_us(ang2)); 57 | delayMicroseconds(1000); 58 | } 59 | 60 | void Robot3R::run_period() { 61 | for (int i = 0; i < 100; ++i){ 62 | for (int j = 0; j < 3; ++j) { 63 | current_theta[j] = task[i][j]; 64 | } 65 | 66 | servo_set_angs(current_theta); 67 | delay(50); 68 | } 69 | } 70 | -------------------------------------------------------------------------------- /hardware_test/motorsTest/motorsTest.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Adafruit_PWMServoDriver.h" 3 | 4 | // called this way, it uses the default address 0x40 5 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); 6 | 7 | void setServoPulse(uint8_t n, double pulse) { 8 | double pulselength; 9 | 10 | pulselength = 1000000; // 1,000,000 us per second 11 | pulselength /= 50; // 50 Hz 12 | Serial.print(pulselength); Serial.println(" us per period"); 13 | pulselength /= 4096; // 12 bits of resolution 14 | Serial.print(pulselength); Serial.println(" us per bit"); 15 | pulse *= 1000; 16 | pulse /= pulselength; 17 | Serial.println(pulse); 18 | pwm.setPWM(n, 0, pulse); 19 | } 20 | 21 | void writeServo(uint8_t n,uint8_t angle){ 22 | double pulse; 23 | pulse=0.5+angle/90.0; 24 | setServoPulse(n,pulse); 25 | } 26 | 27 | void setup() { 28 | Serial.begin(9600); 29 | Serial.println("16 channel PWM test!"); 30 | pwm.begin(); 31 | pwm.setPWMFreq(50); // This is the maximum PWM frequency 32 | } 33 | 34 | String inString = ""; 35 | void loop() { 36 | if (Serial.available() > 0) { 37 | if (Serial.peek() != '\n') { 38 | inString += (char)Serial.read(); 39 | } else { 40 | Serial.read(); 41 | Serial.print("Instruction received: "); 42 | Serial.println(inString); 43 | switch(inString.toInt()) { 44 | case 1: 45 | writeServo(0, 45); 46 | writeServo(1, 90); 47 | writeServo(2, 180); 48 | break; 49 | case 2: 50 | writeServo(4, 145); 51 | writeServo(5, 90); 52 | writeServo(6, 0); 53 | break; 54 | case 3: 55 | writeServo(8, 45); 56 | writeServo(9, 90); 57 | writeServo(10, 180); 58 | break; 59 | case 4: 60 | writeServo(12, 135); 61 | writeServo(13, 90); 62 | writeServo(14, 0); 63 | break; 64 | case 0: 65 | pwm.setPWM(0, 0, 0); 66 | pwm.setPWM(1, 0, 0); 67 | pwm.setPWM(2, 0, 0); 68 | pwm.setPWM(4, 0, 0); 69 | pwm.setPWM(5, 0, 0); 70 | pwm.setPWM(6, 0, 0); 71 | pwm.setPWM(8, 0, 0); 72 | pwm.setPWM(9, 0, 0); 73 | pwm.setPWM(10, 0, 0); 74 | pwm.setPWM(12, 0, 0); 75 | pwm.setPWM(13, 0, 0); 76 | pwm.setPWM(14, 0, 0); 77 | break; 78 | } 79 | inString = ""; 80 | } 81 | } 82 | } 83 | -------------------------------------------------------------------------------- /webots_sim/controllers/cpg_controller/cpg_controller.cpp: -------------------------------------------------------------------------------- 1 | // File: cpg_controller.cpp 2 | // Date: Feb-19-2020 3 | // Description: 4 | // Author: Colin 5 | // Modifications: Feb-19-2020 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #define TIME_STEP 16 12 | #define T 1 13 | 14 | 15 | using namespace webots; 16 | 17 | double deg_2_rad(double x) { 18 | return x / 180.0 * M_PI; 19 | } 20 | 21 | int main(int argc, char **argv) { 22 | // create the Robot instance. 23 | Robot *robot = new Robot(); 24 | 25 | Motor *servos[12]; 26 | char servo_names[12][12] { 27 | "servo_hip1x", "servo_hip1z", "servo_knee1", 28 | "servo_hip2x", "servo_hip2z", "servo_knee2", 29 | "servo_hip3x", "servo_hip3z", "servo_knee3", 30 | "servo_hip4x", "servo_hip4z", "servo_knee4" 31 | }; 32 | for (int i = 0; i < 12; i++) { 33 | servos[i] = robot->getMotor(servo_names[i]); 34 | } 35 | 36 | // Params 37 | double Ah = 10; 38 | double Ak = 15; 39 | double t = 0.0; 40 | 41 | 42 | const double init_angs[12] { 43 | 0, -15, 30, 0, -15, 30, 0, 15, -30, 0, 15, -30 44 | }; 45 | 46 | double angs[12] {}; 47 | for (int i = 0; i < 12; i++) { 48 | angs[i] = init_angs[i]; 49 | servos[i]->setPosition(deg_2_rad(angs[i])); 50 | } 51 | 52 | 53 | 54 | while (robot->step(TIME_STEP) != -1) { 55 | angs[0] = 0 + init_angs[0]; 56 | angs[1] = Ah * sin(2 * M_PI / T * t - M_PI / 2) + init_angs[1]; 57 | 58 | double knee1 = Ak * sin(2 * M_PI / T * t ); 59 | double knee2 = Ak * sin(2 * M_PI / T * t + M_PI); 60 | double knee3 = Ak * sin(2 * M_PI / T * t); 61 | double knee4 = Ak * sin(2 * M_PI / T * t + M_PI); 62 | 63 | if (knee1 < 0) knee1 = 0; 64 | if (knee2 < 0) knee2 = 0; 65 | if (knee3 < 0) knee3 = 0; 66 | if (knee4 < 0) knee4 = 0; 67 | 68 | angs[2] = knee1 + init_angs[2]; 69 | angs[3] = 0 + init_angs[3]; 70 | angs[4] = Ah * sin(2 * M_PI / T * t + M_PI / 2) + init_angs[4]; 71 | angs[5] = knee2 + init_angs[5]; 72 | angs[6] = 0 + init_angs[6]; 73 | angs[7] = Ah * sin(2 * M_PI / T * t - M_PI / 2) + init_angs[7]; 74 | angs[8] = -knee3 + init_angs[8]; 75 | angs[9] = 0 + init_angs[9]; 76 | angs[10] = Ah * sin(2 * M_PI / T * t + M_PI / 2) + init_angs[10]; 77 | angs[11] = -knee4 + init_angs[11]; 78 | 79 | for (int i = 0; i < 12; i++) { 80 | servos[i]->setPosition(deg_2_rad(angs[i])); 81 | } 82 | 83 | 84 | t += (double)TIME_STEP / 1000.0; 85 | }; 86 | 87 | 88 | // Enter here exit cleanup code. 89 | 90 | delete robot; 91 | return 0; 92 | } 93 | -------------------------------------------------------------------------------- /hardware_test/LabRobot/StableGait/test/Actuator.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "Actuator.h" 3 | #include "Adafruit_PWMServoDriver.h" 4 | 5 | 6 | /** 7 | * 8 | * ^ Y 9 | * | 10 | * back |---> X forward 11 | * Z 12 | * 13 | */ 14 | Actuator::Actuator() 15 | { 16 | // init servo driver, default address 0x40 17 | pwm = new Adafruit_PWMServoDriver(); 18 | 19 | } 20 | 21 | 22 | void Actuator::init() 23 | { 24 | Serial.println("init pwm driver"); 25 | pwm->begin(); 26 | pwm->setPWMFreq(freq); 27 | Serial.println("init legs"); 28 | legFL = new Leg(); 29 | legFR = new Leg(); 30 | legRR = new Leg(); 31 | legRL = new Leg(); 32 | LegPtr legs[4] {legFL, legFR, legRR, legRL}; 33 | // 1 the same with axis, -1 the opposite 34 | const int8_t servo_dir[12]{1, -1, -1, 1, 1, 1, -1, 1, 1, -1, -1, -1}; 35 | const float init_ang[12]{35, 90, 180, 125, 83, 5, 40, 85, 180, 127, 83, 0}; 36 | for (uint8_t i = 0; i < 4; i++) 37 | { 38 | legs[i]->sRoll = new Servo(i * 3, servo_dir[i * 3], init_ang[i * 3]); 39 | legs[i]->sPitch = new Servo(i * 3 + 1, servo_dir[i * 3 + 1], init_ang[i * 3 + 1]); 40 | legs[i]->sKnee = new Servo(i * 3 + 2, servo_dir[i * 3 + 2], init_ang[i * 3 + 2]); 41 | } 42 | } 43 | 44 | 45 | void Actuator::set_servo_pulse(ServoPtr servo, double pulse) 46 | { 47 | double pulseleng; 48 | pulseleng = 1000000; // 1,000,000 per second 49 | pulseleng /= freq; 50 | pulseleng /= resolution; 51 | pulse *= 1000; // convert to us 52 | pulse /= pulseleng; 53 | pwm->setPWM(servo->id, 0, pulse); 54 | } 55 | 56 | 57 | void Actuator::write_servo_ang(ServoPtr servo , float angle) 58 | { 59 | double pulse; 60 | pulse = 0.5 + angle / 90.0; 61 | set_servo_pulse(servo, pulse); 62 | servo->angle = angle; 63 | } 64 | 65 | 66 | void Actuator::adjust() { 67 | // adjust front left leg 68 | write_servo_ang(legFL->sRoll, legFL->sRoll->init_ang); 69 | write_servo_ang(legFL->sPitch, legFL->sPitch->init_ang); 70 | write_servo_ang(legFL->sKnee, legFL->sKnee->init_ang); 71 | 72 | write_servo_ang(legFR->sRoll, legFR->sRoll->init_ang); 73 | write_servo_ang(legFR->sPitch, legFR->sPitch->init_ang); 74 | write_servo_ang(legFR->sKnee, legFR->sKnee->init_ang); 75 | 76 | write_servo_ang(legRR->sRoll, legRR->sRoll->init_ang); 77 | write_servo_ang(legRR->sPitch, legRR->sPitch->init_ang); 78 | write_servo_ang(legRR->sKnee, legRR->sKnee->init_ang); 79 | 80 | write_servo_ang(legRL->sRoll, legRL->sRoll->init_ang); 81 | write_servo_ang(legRL->sPitch, legRL->sPitch->init_ang); 82 | write_servo_ang(legRL->sKnee, legRL->sKnee->init_ang); 83 | } 84 | 85 | 86 | Actuator::~Actuator() 87 | { 88 | delete pwm; 89 | pwm = nullptr; 90 | } 91 | -------------------------------------------------------------------------------- /mathematics/inverse_kinematic/results/targetPoints: -------------------------------------------------------------------------------- 1 | X,Y 2 | 0.0,-19 3 | -0.25,-19 4 | -0.5,-19 5 | -0.75,-19 6 | -1.0,-19 7 | -1.25,-19 8 | -1.5,-19 9 | -1.75,-19 10 | -2.0,-19 11 | -2.25,-19 12 | -2.5,-19 13 | -2.75,-19 14 | -3.0,-19 15 | -3.25,-19 16 | -3.5,-19 17 | -3.75,-19 18 | -4.0,-19 19 | -4.25,-19 20 | -4.5,-19 21 | -4.75,-19 22 | -5.0,-19 23 | -5.25,-19 24 | -5.5,-19 25 | -5.75,-19 26 | -6.0,-19 27 | -6.00000000000000,-19.0000000000000 28 | -5.87313480978314,-18.7149887780992 29 | -5.73274039506625,-18.4363930469501 30 | -5.57914016534305,-18.1648545735671 31 | -5.41268795073295,-17.9009988680204 32 | -5.23376718690583,-17.6454337425246 33 | -5.04279003180797,-17.3987479112961 34 | -4.84019641622412,-17.1615096344038 35 | -4.62645303036252,-16.9342654087377 36 | -4.40205224879765,-16.7175387091106 37 | -4.16751099624689,-16.5118287823932 38 | -3.92336955679413,-16.3176094974596 39 | -3.67019032930321,-16.1353282535938 40 | -3.40855653188819,-15.9654049498701 41 | -3.13907085842495,-15.8082310178832 42 | -2.86235409019876,-15.6641685200545 43 | -2.57904366588616,-15.5335493155937 44 | -2.28979221316518,-15.4166742960344 45 | -1.99526604533658,-15.3138126921078 46 | -1.69614362641909,-15.2252014535479 47 | -1.39311400825464,-15.1510447032599 48 | -1.08687524322361,-15.0915132671068 49 | -0.778132776226706,-15.0467442803991 50 | -0.467597819637559,-15.0168408719923 51 | -0.155985714969491,-15.0018719267219 52 | 0.155985714969487,-15.0018719267219 53 | 0.467597819637554,-15.0168408719923 54 | 0.778132776226702,-15.0467442803991 55 | 1.08687524322360,-15.0915132671068 56 | 1.39311400825463,-15.1510447032599 57 | 1.69614362641909,-15.2252014535479 58 | 1.99526604533657,-15.3138126921078 59 | 2.28979221316518,-15.4166742960344 60 | 2.57904366588615,-15.5335493155937 61 | 2.86235409019875,-15.6641685200545 62 | 3.13907085842494,-15.8082310178832 63 | 3.40855653188818,-15.9654049498701 64 | 3.67019032930321,-16.1353282535938 65 | 3.92336955679413,-16.3176094974596 66 | 4.16751099624689,-16.5118287823932 67 | 4.40205224879765,-16.7175387091106 68 | 4.62645303036252,-16.9342654087377 69 | 4.84019641622412,-17.1615096344038 70 | 5.04279003180797,-17.3987479112961 71 | 5.23376718690582,-17.6454337425246 72 | 5.41268795073295,-17.9009988680204 73 | 5.57914016534304,-18.1648545735671 74 | 5.73274039506625,-18.4363930469501 75 | 5.87313480978313,-18.7149887780992 76 | 6.00000000000000,-19.0000000000000 77 | 6.0,-19 78 | 5.75,-19 79 | 5.5,-19 80 | 5.25,-19 81 | 5.0,-19 82 | 4.75,-19 83 | 4.5,-19 84 | 4.25,-19 85 | 4.0,-19 86 | 3.75,-19 87 | 3.5,-19 88 | 3.25,-19 89 | 3.0,-19 90 | 2.75,-19 91 | 2.5,-19 92 | 2.25,-19 93 | 2.0,-19 94 | 1.75,-19 95 | 1.5,-19 96 | 1.25,-19 97 | 1.0,-19 98 | 0.75,-19 99 | 0.5,-19 100 | 0.25,-19 101 | 0.0,-19 102 | -------------------------------------------------------------------------------- /hardware_test/LabRobot/DogRobot/QuadrupedRobot.h: -------------------------------------------------------------------------------- 1 | #ifndef QUADRUPED_ROBOT_H 2 | #define QUADRUPED_ROBOT_H 3 | 4 | #include "Arduino.h" 5 | #include 6 | 7 | #define PT(s) Serial.print(s) 8 | #define PTL(s) Serial.println(s) 9 | #define PTF(s) Serial.print(F(s)) //trade flash memory for dynamic memory with F() function 10 | #define PTLF(s) Serial.println(F(s)) 11 | 12 | // Servo Pin configs 13 | #define rFL_PIN 3 14 | #define sFL_PIN 4 15 | #define kFL_PIN 5 16 | #define rFR_PIN 6 17 | #define sFR_PIN 7 18 | #define kFR_PIN 8 19 | #define rHR_PIN 9 20 | #define sHR_PIN 10 21 | #define kHR_PIN 11 22 | #define rHL_PIN 12 23 | #define sHL_PIN 13 24 | #define kHL_PIN 14 25 | 26 | //servo constants 27 | #define SERVOMIN 500 28 | #define SERVOMAX 2500 29 | 30 | #define DOF 12 31 | 32 | // CPG Oscillator data output time period: CPG_DT * CPG_ITERATES 33 | #define CPG_DT 0.005 34 | #define CPG_ITERATES 10 35 | 36 | class QuadrupedRobot 37 | { 38 | private: 39 | Servo *servos[DOF] = { 40 | nullptr, nullptr, nullptr, 41 | nullptr, nullptr, nullptr, 42 | nullptr, nullptr, nullptr, 43 | nullptr, nullptr, nullptr}; 44 | byte pins[DOF]{ 45 | rFL_PIN, sFL_PIN, kFL_PIN, 46 | rFR_PIN, sFR_PIN, kFR_PIN, 47 | rHR_PIN, sHR_PIN, kHR_PIN, 48 | rHL_PIN, sHL_PIN, kHL_PIN}; 49 | 50 | // Servo rotation configs 51 | uint8_t init_angs[12]{35, 90, 180, 130, 83, 0, 35, 85, 180, 130, 83, 0}; 52 | int8_t dir[12]{1, -1, -1, -1, 1, 1, -1, 1, 1, 1, -1, -1}; 53 | 54 | // Angles in callibrated coordinate 55 | double stand_angs[12]{0, -30, 60, 0, -30, 60, 0, 30, -60, 0, 30, -60}; 56 | double rest_angs[12]{0, -55, 130, 0, -55, 130, 0, 55, -130, 0, 55, -130}; 57 | double *current_angs_ptr{nullptr}; 58 | 59 | /** 60 | * CPG Gait Configs 61 | */ 62 | uint8_t alpha{1000}; 63 | double beta{0.5}; 64 | double Ah{0.2}; 65 | double Ak{0.2}; 66 | uint8_t a{10}; 67 | double omega_sw{5*M_PI}; 68 | 69 | // oscillator signals 70 | double x[4] {1, 0, 0, 0}; 71 | double y[4] {0, 0, 0, 0}; 72 | // x_dot = dx / dt 73 | double x_dot[4] {}; 74 | double y_dot[4] {}; 75 | // r_square = x^2 + y^2 76 | double r_square[4] {}; 77 | 78 | double omega_st {}; 79 | double mu {}; 80 | double omega[4] {}; 81 | double theta[4][4] {}; 82 | 83 | double rad2deg(double rad); 84 | void setup_servos(); 85 | void shut_servos(); 86 | void servo_write_angs(); 87 | void cpg_signal(); 88 | 89 | public: 90 | QuadrupedRobot(); 91 | ~QuadrupedRobot(); 92 | 93 | // lock any robot motion 94 | bool lock{false}; 95 | 96 | void switch_on(); 97 | void switch_off(); 98 | void adjust(); 99 | void bot_rest(); 100 | void bot_stand(); 101 | void bot_walk(); 102 | void bot_trot(); 103 | 104 | }; 105 | 106 | #endif 107 | -------------------------------------------------------------------------------- /webots_sim/controllers/init_testing/init_testing.py: -------------------------------------------------------------------------------- 1 | """init_testing controller.""" 2 | 3 | from controller import Robot 4 | import math 5 | import numpy as np 6 | 7 | 8 | def setPositions(motors, theta, posSensors): 9 | """ 10 | set joint positions 11 | not feedback control 12 | """ 13 | delta = 0.01 14 | for i in range(len(motors)): 15 | motors[i].setPosition(theta[i]) 16 | 17 | while robot.step(timestep) != -1: 18 | checked = True 19 | for i in range(len(motors)): 20 | effective = posSensors[i].getValue() 21 | target = theta[i] 22 | 23 | if math.fabs(effective - target) > delta: 24 | # print('i th part: ', i) 25 | # print('error: ', effective - target) 26 | checked = False 27 | if checked: 28 | break 29 | 30 | 31 | def setVelocity(motors, v): 32 | """ 33 | v: rad/s 34 | """ 35 | for i in range(len(motors)): 36 | motors[i].setVelocity(v) 37 | 38 | 39 | # create the Robot instance. 40 | robot = Robot() 41 | 42 | # get the time step of the current world. 43 | timestep = int(robot.getBasicTimeStep()) 44 | 45 | # init 12 DOF motors and position sensors 46 | motorNames = [ 47 | 'hip0x', 'hip0z', 'knee0', 48 | 'hip1x', 'hip1z', 'knee1', 49 | 'hip2x', 'hip2z', 'knee2', 50 | 'hip3x', 'hip3z', 'knee3' 51 | ] 52 | motors = [ 53 | robot.getMotor(motorNames[i]) for i in range(len(motorNames)) 54 | ] 55 | posSensors = [ 56 | robot.getPositionSensor(motorNames[i] + '_pos') for i in range(len(motorNames)) 57 | ] 58 | for posSensor in posSensors: 59 | posSensor.enable(timestep) 60 | 61 | # init positions 62 | thetaInit = np.array([ 63 | 0, -45/180 * math.pi, 90/180 * math.pi, 64 | 0, -45/180 * math.pi, 90/180 * math.pi, 65 | 0, 45/180 * math.pi, -90/180 * math.pi, 66 | 0, 45/180 * math.pi, -90/180 * math.pi 67 | ]) 68 | setPositions(motors, thetaInit, posSensors) 69 | setVelocity(motors, 10) 70 | 71 | isLeftStepUp = True 72 | leftStepUp = [ 73 | 0, -45/180 * math.pi, 90/180 * math.pi, 74 | 0, -55/180 * math.pi, 110/180 * math.pi, 75 | 0, 55/180 * math.pi, -110/180 * math.pi, 76 | 0, 45/180 * math.pi, -90/180 * math.pi 77 | ] 78 | rightStepUp = [ 79 | 0, -55/180 * math.pi, 110/180 * math.pi, 80 | 0, -45/180 * math.pi, 90/180 * math.pi, 81 | 0, 45/180 * math.pi, -90/180 * math.pi, 82 | 0, 55/180 * math.pi, -110/180 * math.pi 83 | ] 84 | 85 | setPositions(motors, leftStepUp, posSensors); 86 | # Main loop: 87 | # - perform simulation steps until Webots is stopping the controller 88 | while robot.step(timestep) != -1: 89 | # if isLeftStepUp == True: 90 | # setPositions(motors, leftStepUp, posSensors) 91 | # else: 92 | # setPositions(motors, rightStepUp, posSensors) 93 | # isLeftStepUp = not isLeftStepUp 94 | 95 | 96 | pass 97 | 98 | # Enter here exit cleanup code. 99 | 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /hardware_test/singleLeg/Robot3R.h: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * Author: Haoxiang Li (Colin) 3 | * 3R Robot 4 | * Header file 5 | *******************************************************************/ 6 | #ifndef ROBOT_3R_H 7 | #define ROBOT_3R_H 8 | 9 | const int SERVO_US_MIN {500}; 10 | const int SERVO_US_MAX {2500}; 11 | // set init angles as the base angles 12 | const uint8_t INIT_ANG[3] {135, 90, 0}; 13 | const int8_t SERVO_DIR[3] {-1, -1, 1}; 14 | 15 | class Robot3R 16 | { 17 | private: 18 | byte hip0_pin {}; 19 | byte hip1_pin {}; 20 | byte knee_pin {}; 21 | int16_t current_theta [3] {0, 16, 36}; 22 | int8_t task[100][3] 23 | { 24 | {0,16,36}, 25 | {0,17,36}, 26 | {0,17,36}, 27 | {0,18,36}, 28 | {0,19,36}, 29 | {0,19,35}, 30 | {0,20,35}, 31 | {0,20,35}, 32 | {0,21,34}, 33 | {0,22,34}, 34 | {0,22,33}, 35 | {0,22,32}, 36 | {0,23,31}, 37 | {0,23,31}, 38 | {0,23,30}, 39 | {0,24,29}, 40 | {0,24,27}, 41 | {0,24,26}, 42 | {0,24,25}, 43 | {0,24,23}, 44 | {0,24,21}, 45 | {0,24,19}, 46 | {0,23,17}, 47 | {0,23,14}, 48 | {0,22,9}, 49 | {0,22,9}, 50 | {0,27,22}, 51 | {0,30,30}, 52 | {0,33,36}, 53 | {0,35,41}, 54 | {0,37,46}, 55 | {0,38,50}, 56 | {0,39,54}, 57 | {0,40,57}, 58 | {0,41,60}, 59 | {0,42,63}, 60 | {0,42,66}, 61 | {0,43,68}, 62 | {0,43,70}, 63 | {0,43,73}, 64 | {0,43,74}, 65 | {0,43,76}, 66 | {0,42,78}, 67 | {0,42,79}, 68 | {0,41,80}, 69 | {0,41,81}, 70 | {0,40,82}, 71 | {0,39,82}, 72 | {0,38,83}, 73 | {0,37,83}, 74 | {0,35,83}, 75 | {0,34,83}, 76 | {0,33,82}, 77 | {0,31,82}, 78 | {0,30,81}, 79 | {0,29,80}, 80 | {0,27,79}, 81 | {0,25,78}, 82 | {0,24,76}, 83 | {0,22,74}, 84 | {0,21,73}, 85 | {0,19,70}, 86 | {0,17,68}, 87 | {0,15,66}, 88 | {0,14,63}, 89 | {0,12,60}, 90 | {0,10,57}, 91 | {0,8,54}, 92 | {0,6,50}, 93 | {0,4,46}, 94 | {0,1,41}, 95 | {0,0,36}, 96 | {0,-3,30}, 97 | {0,-7,22}, 98 | {0,-13,9}, 99 | {0,-13,9}, 100 | {0,-10,14}, 101 | {0,-8,17}, 102 | {0,-6,19}, 103 | {0,-5,21}, 104 | {0,-3,23}, 105 | {0,-2,25}, 106 | {0,0,26}, 107 | {0,0,27}, 108 | {0,1,29}, 109 | {0,3,30}, 110 | {0,4,31}, 111 | {0,5,31}, 112 | {0,6,32}, 113 | {0,7,33}, 114 | {0,8,34}, 115 | {0,9,34}, 116 | {0,10,35}, 117 | {0,11,35}, 118 | {0,12,35}, 119 | {0,13,36}, 120 | {0,14,36}, 121 | {0,14,36}, 122 | {0,15,36}, 123 | {0,16,36}, 124 | }; 125 | 126 | 127 | public: 128 | Robot3R(byte hip0_pin, byte hip1_pin, byte knee_pin); 129 | void start(); 130 | int convert_rad_to_pulse_us(double rad); 131 | int convert_ang_to_pulse_us(int16_t ang); 132 | void servo_set_angs(int16_t angs[]); 133 | void run_period(); 134 | }; 135 | #endif 136 | -------------------------------------------------------------------------------- /hardware_test/LabRobot/motorsTest/motorsTest.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | Servo s1, s2, s3, s4, s5, s6, s7, s8, s9, s10, s11, s12; 4 | 5 | double ang2pulse(uint8_t angle) { 6 | double pulse; 7 | pulse = (0.5 + angle/90.0) * 1000; 8 | return pulse; 9 | } 10 | 11 | void setup_servos() { 12 | Serial.println("Setup Servos"); 13 | // limb1 14 | s1.attach(3, 500, 2500); 15 | s2.attach(4, 500, 2500); 16 | s3.attach(5, 500, 2500); 17 | // limb2 18 | s4.attach(6, 500, 2500); 19 | s5.attach(7, 500, 2500); 20 | s6.attach(8, 500, 2500); 21 | // limb3 22 | s7.attach(9, 500, 2500); 23 | s8.attach(10, 500, 2500); 24 | s9.attach(11, 500, 2500); 25 | // limb4 26 | s10.attach(12, 500, 2500); 27 | s11.attach(13, 500, 2500); 28 | s12.attach(14, 500, 2500); 29 | } 30 | 31 | void shut_servos() { 32 | Serial.println("Shut Servos"); 33 | s1.detach(); 34 | s2.detach(); 35 | s3.detach(); 36 | s4.detach(); 37 | s5.detach(); 38 | s6.detach(); 39 | s7.detach(); 40 | s8.detach(); 41 | s9.detach(); 42 | s10.detach(); 43 | s11.detach(); 44 | s12.detach(); 45 | } 46 | 47 | void run_servos() { 48 | Serial.println("Run Servos"); 49 | s1.writeMicroseconds(ang2pulse(45)); 50 | s2.writeMicroseconds(ang2pulse(90)); 51 | s3.writeMicroseconds(ang2pulse(180)); 52 | 53 | s4.writeMicroseconds(ang2pulse(130)); 54 | s5.writeMicroseconds(ang2pulse(83)); 55 | s6.writeMicroseconds(ang2pulse(0)); 56 | 57 | s7.writeMicroseconds(ang2pulse(45)); 58 | s8.writeMicroseconds(ang2pulse(90)); 59 | s9.writeMicroseconds(ang2pulse(180)); 60 | 61 | s10.writeMicroseconds(ang2pulse(130)); 62 | s11.writeMicroseconds(ang2pulse(83)); 63 | s12.writeMicroseconds(ang2pulse(0)); 64 | 65 | 66 | 67 | // s1.writeMicroseconds(ang2pulse(45)); 68 | // s2.writeMicroseconds(ang2pulse(90)); 69 | // s3.writeMicroseconds(ang2pulse(180)); 70 | 71 | delay(1000); 72 | } 73 | 74 | void setup() { 75 | Serial.begin(9600); 76 | Serial.setTimeout(10); 77 | while (!Serial); 78 | // wait for ready 79 | while (Serial.available() && Serial.read()); // empty buffer 80 | 81 | Serial.println("Lab robot servo test!"); 82 | Serial.println("Please select code:"); 83 | Serial.println("1. Setup Servo"); 84 | Serial.println("2. Run"); 85 | Serial.println("3. Shut Servo"); 86 | } 87 | 88 | String inString = ""; 89 | void loop() { 90 | if (Serial.available() > 0) { 91 | if (Serial.peek() != '\n') { 92 | inString += (char)Serial.read(); 93 | } else { 94 | Serial.read(); 95 | Serial.print("Instruction received: "); 96 | 97 | int code = inString.toInt(); 98 | Serial.println(code); 99 | switch(code) { 100 | case 1: 101 | setup_servos(); 102 | break; 103 | case 2: 104 | run_servos(); 105 | break; 106 | case 3: 107 | shut_servos(); 108 | break; 109 | } 110 | inString = ""; 111 | } 112 | } 113 | 114 | } 115 | -------------------------------------------------------------------------------- /webots_sim/controllers/cpg_controller/Makefile: -------------------------------------------------------------------------------- 1 | # Copyright 1996-2019 Cyberbotics Ltd. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | ### Generic Makefile.include for Webots controllers, physics plugins, robot 16 | ### window libraries, remote control libraries and other libraries 17 | ### to be used with GNU make 18 | ### 19 | ### Platforms: Windows, macOS, Linux 20 | ### Languages: C, C++ 21 | ### 22 | ### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer 23 | ### Edmund Ronald, Sergei Poskriakov 24 | ### 25 | ###----------------------------------------------------------------------------- 26 | ### 27 | ### This file is meant to be included from the Makefile files located in the 28 | ### Webots projects subdirectories. It is possible to set a number of variables 29 | ### to customize the build process, i.e., add source files, compilation flags, 30 | ### include paths, libraries, etc. These variables should be set in your local 31 | ### Makefile just before including this Makefile.include. This Makefile.include 32 | ### should never be modified. 33 | ### 34 | ### Here is a description of the variables you may set in your local Makefile: 35 | ### 36 | ### ---- C Sources ---- 37 | ### if your program uses several C source files: 38 | ### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c 39 | ### 40 | ### ---- C++ Sources ---- 41 | ### if your program uses several C++ source files: 42 | ### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++ 43 | ### 44 | ### ---- Compilation options ---- 45 | ### if special compilation flags are necessary: 46 | ### CFLAGS = -Wno-unused-result 47 | ### 48 | ### ---- Linked libraries ---- 49 | ### if your program needs additional libraries: 50 | ### INCLUDE = -I"/my_library_path/include" 51 | ### LIBRARIES = -L"/path/to/my/library" -lmy_library -lmy_other_library 52 | ### 53 | ### ---- Linking options ---- 54 | ### if special linking flags are needed: 55 | ### LFLAGS = -s 56 | ### 57 | ### ---- Webots included libraries ---- 58 | ### if you want to use the Webots C API in your C++ controller program: 59 | ### USE_C_API = true 60 | ### if you want to link with the Qt framework embedded in Webots: 61 | ### QT = core gui widgets network 62 | ### 63 | ### ---- Debug mode ---- 64 | ### if you want to display the gcc command line for compilation and link, as 65 | ### well as the rm command details used for cleaning: 66 | ### VERBOSE = 1 67 | ### 68 | ###----------------------------------------------------------------------------- 69 | 70 | ### Do not modify: this includes Webots global Makefile.include 71 | space := 72 | space += 73 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 74 | include $(WEBOTS_HOME_PATH)/resources/Makefile.include 75 | -------------------------------------------------------------------------------- /webots_sim/controllers/cpg_hopf_controller/Makefile: -------------------------------------------------------------------------------- 1 | # Copyright 1996-2019 Cyberbotics Ltd. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | ### Generic Makefile.include for Webots controllers, physics plugins, robot 16 | ### window libraries, remote control libraries and other libraries 17 | ### to be used with GNU make 18 | ### 19 | ### Platforms: Windows, macOS, Linux 20 | ### Languages: C, C++ 21 | ### 22 | ### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer 23 | ### Edmund Ronald, Sergei Poskriakov 24 | ### 25 | ###----------------------------------------------------------------------------- 26 | ### 27 | ### This file is meant to be included from the Makefile files located in the 28 | ### Webots projects subdirectories. It is possible to set a number of variables 29 | ### to customize the build process, i.e., add source files, compilation flags, 30 | ### include paths, libraries, etc. These variables should be set in your local 31 | ### Makefile just before including this Makefile.include. This Makefile.include 32 | ### should never be modified. 33 | ### 34 | ### Here is a description of the variables you may set in your local Makefile: 35 | ### 36 | ### ---- C Sources ---- 37 | ### if your program uses several C source files: 38 | ### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c 39 | ### 40 | ### ---- C++ Sources ---- 41 | ### if your program uses several C++ source files: 42 | ### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++ 43 | ### 44 | ### ---- Compilation options ---- 45 | ### if special compilation flags are necessary: 46 | ### CFLAGS = -Wno-unused-result 47 | ### 48 | ### ---- Linked libraries ---- 49 | ### if your program needs additional libraries: 50 | ### INCLUDE = -I"/my_library_path/include" 51 | ### LIBRARIES = -L"/path/to/my/library" -lmy_library -lmy_other_library 52 | ### 53 | ### ---- Linking options ---- 54 | ### if special linking flags are needed: 55 | ### LFLAGS = -s 56 | ### 57 | ### ---- Webots included libraries ---- 58 | ### if you want to use the Webots C API in your C++ controller program: 59 | ### USE_C_API = true 60 | ### if you want to link with the Qt framework embedded in Webots: 61 | ### QT = core gui widgets network 62 | ### 63 | ### ---- Debug mode ---- 64 | ### if you want to display the gcc command line for compilation and link, as 65 | ### well as the rm command details used for cleaning: 66 | ### VERBOSE = 1 67 | ### 68 | ###----------------------------------------------------------------------------- 69 | 70 | ### Do not modify: this includes Webots global Makefile.include 71 | space := 72 | space += 73 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 74 | include $(WEBOTS_HOME_PATH)/resources/Makefile.include 75 | -------------------------------------------------------------------------------- /mathematics/inverse_kinematic/taskspace.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import pandas as pd 4 | from sympy import * 5 | 6 | 7 | def validate(l1, l2, tri_x, tri_y): 8 | if tri_x ** 2 + tri_y ** 2 >= (l1 + l2) ** 2: 9 | print('out of the workspace!') 10 | if tri_x ** 2 + tri_y ** 2 <= (l1 - l2) ** 2: 11 | print('out of the workspace!') 12 | 13 | 14 | def find_circle_origin(p1, p2, p3): 15 | x, y = symbols('x y') 16 | p1x, p1y = p1 17 | p2x, p2y = p2 18 | p3x, p3y = p3 19 | eq1 = Eq((x - p1x) ** 2 + (y - p1y) ** 2, (x - p2x) ** 2 + (y - p2y) ** 2) 20 | eq2 = Eq((x - p1x) ** 2 + (y - p1y) ** 2, (x - p3x) ** 2 + (y - p3y) ** 2) 21 | res = linsolve((eq1, eq2), (x, y)) 22 | return res.args[0] 23 | 24 | 25 | # length of links (cm) 26 | link1 = 11 27 | link2 = 9 28 | # angle range limits 29 | theta1 = np.arange(0., np.pi / 2, 0.01) 30 | theta2 = np.arange(0., np.pi, 0.01) 31 | 32 | # point O, Hip1 33 | o = (0, 0) 34 | ox, oy = o 35 | 36 | a = [] 37 | b = [] 38 | 39 | steps = 100 40 | for theta1 in np.linspace(np.pi / 4, np.pi * 3 / 4, steps): 41 | for theta2 in np.linspace(0., np.pi, steps): 42 | # point A, Knee 43 | ax = ox - link1 * np.cos(theta1) 44 | ay = oy - link1 * np.sin(theta1) 45 | a.append([ax, ay]) 46 | # point B, End effect 47 | bx = ax + link2 * np.cos(theta1 + theta2) 48 | by = ay + link2 * np.sin(theta1 + theta2) 49 | b.append([bx, by]) 50 | 51 | # transfer to matrix 52 | a = np.array(a) 53 | b = np.array(b) 54 | 55 | fig, ax = plt.subplots() 56 | plt.title('End Effect Task Space') 57 | 58 | wx, wy = b.transpose() 59 | ax.scatter(wx, wy, alpha=0.2) 60 | 61 | 62 | # route design (manually) 63 | # duty circle: 0.5 64 | # form: hemisphere 65 | # position: right under the origin with high h 66 | # phases: 4 67 | fh = 1 68 | phases = 4 69 | # step length and height 70 | deltaS = 12 71 | deltaH = 4 72 | # counting stops 73 | routeSteps = 100 74 | phaseSteps = routeSteps // phases 75 | 76 | # high of the robot in ready state (cm) 77 | h = link1 + link2 - fh 78 | 79 | # phase 1, straight line 80 | rx1, ry1 = (ox, -h) 81 | # phase 2 an 3, curve 82 | rx2, ry2 = (ox - deltaS // 2, -h) 83 | rx3, ry3 = (ox, oy - h + deltaH) 84 | # phase 4, straight line 85 | rx4, ry4 = (ox + deltaS // 2, -h) 86 | 87 | rx = np.linspace(rx1, ox - deltaS // 2, phaseSteps) 88 | ry = np.repeat(ry1, phaseSteps) 89 | 90 | cx, cy = find_circle_origin((rx2, ry2), (rx3, ry3), (rx4, ry4)) 91 | print('cx, cy: ', cx, cy) 92 | angHalf = np.pi - 2 * np.arctan(deltaS // 2 / deltaH) 93 | R = sqrt((cx - rx2) ** 2 + (cy - ry2) ** 2) 94 | for theta in np.linspace(np.pi / 2 - angHalf, np.pi / 2 + angHalf, phaseSteps * 2): 95 | rxNew = cx - R * np.cos(theta) 96 | ryNew = cy + R * np.sin(theta) 97 | validate(link1, link2, rxNew - ox, ryNew - oy) 98 | rx = np.append(rx, [rxNew]) 99 | ry = np.append(ry, [ryNew]) 100 | 101 | rx = np.append(rx, np.linspace(ox + deltaS // 2, ox, phaseSteps)) 102 | ry = np.append(ry, np.repeat(-h, phaseSteps)) 103 | 104 | plt.plot(rx, ry, 'y') 105 | targets = pd.DataFrame({'X': rx, 'Y': ry}) 106 | targets.to_csv('./results/targetPoints', index=False) 107 | 108 | # plot annotations 109 | ax.scatter([rx1, rx2, rx3, rx4], [ry1, ry2, ry3, ry4], alpha=1) 110 | ax.scatter(ox, oy, alpha=1) 111 | 112 | ax.annotate('O', (ox, oy + 0.5)) 113 | ax.annotate('A', (rx1, ry1 + 0.5)) 114 | ax.annotate('B', (rx2 - 0.8, ry2 + 0.5)) 115 | ax.annotate('C', (rx3, ry3 + 0.5)) 116 | ax.annotate('D', (rx4, ry4 + 0.5)) 117 | 118 | plt.show() 119 | 120 | 121 | 122 | 123 | -------------------------------------------------------------------------------- /webots_sim/controllers/cpg_hopf_controller/cpg_hopf_controller.cpp: -------------------------------------------------------------------------------- 1 | // File: cpg_hopf_controller.cpp 2 | // Date: Feb-20-2020 3 | // Description: 4 | // Author: Colin 5 | // Modifications: Feb-20-2020 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #define alpha 100 12 | #define beta 0.5 13 | #define Ah 0.2 14 | #define Ak 0.1 15 | #define a 10 16 | #define omega_sw 3 * M_PI 17 | #define phi_LF 0 18 | #define phi_RF 0.5 19 | #define phi_LH 0.5 20 | #define phi_RH 0 21 | 22 | 23 | // All the webots classes are defined in the "webots" namespace 24 | using namespace webots; 25 | 26 | double deg_2_rad(double x) { 27 | return x / 180.0 * M_PI; 28 | } 29 | 30 | int main(int argc, char **argv) { 31 | // create the Robot instance. 32 | Robot *robot = new Robot(); 33 | 34 | // get the time step of the current world. 35 | int timeStep = (int)robot->getBasicTimeStep(); 36 | 37 | Motor *servos[12]; 38 | char servo_names[12][12] { 39 | "servo_hip1x", "servo_hip1z", "servo_knee1", 40 | "servo_hip2x", "servo_hip2z", "servo_knee2", 41 | "servo_hip3x", "servo_hip3z", "servo_knee3", 42 | "servo_hip4x", "servo_hip4z", "servo_knee4" 43 | }; 44 | for (int i = 0; i < 12; i++) { 45 | servos[i] = robot->getMotor(servo_names[i]); 46 | } 47 | 48 | const double init_angs[12] { 49 | 0, -15, 30, 0, -15, 30, 0, 15, -30, 0, 15, -30 50 | }; 51 | double angs[12] {}; 52 | for (int i = 0; i < 12; i++) { 53 | angs[i] = init_angs[i]; 54 | servos[i]->setPosition(deg_2_rad(angs[i])); 55 | } 56 | 57 | /** 58 | * intermediate params 59 | */ 60 | // oscillator signals 61 | double x[4] {1, 0, 0, 0}; 62 | double y[4] {0, 0, 0, 0}; 63 | // x_dot = dx / dt 64 | double x_dot[4] {}; 65 | double y_dot[4] {}; 66 | // r_square = x^2 + y^2 67 | double r_square[4] {}; 68 | 69 | double omega_st {}; 70 | double mu {}; 71 | double phi[4] {phi_LF, phi_RF, phi_RH, phi_LH}; 72 | double omega[4] {}; 73 | double theta[4][4] {}; 74 | double dt {}; 75 | 76 | dt = (double)timeStep / 2000; 77 | omega_st = omega_sw * (1 - beta) / beta; 78 | mu = Ah * Ah; 79 | for (int i = 0; i < 4; i++) { 80 | for (int j = 0; j < 4; j++) { 81 | theta[i][j] = 2 * M_PI * (phi[i] - phi[j]); 82 | } 83 | } 84 | 85 | // Main Loop 86 | while (robot->step(timeStep) != -1) { 87 | 88 | for (int i = 0; i < 4; i++) { 89 | r_square[i] = x[i] * x[i] + y[i] * y[i]; 90 | 91 | // Frequency of the oscillator 92 | omega[i] = omega_st / (exp(- a * y[i]) + 1) + omega_sw / (exp(a * y[i]) + 1); 93 | 94 | // HOPF Oscillator 95 | x_dot[i] = alpha * (mu - r_square[i]) * x[i] - omega[i] * y[i]; 96 | y_dot[i] = alpha * (mu - r_square[i]) * y[i] + omega[i] * x[i]; 97 | 98 | /** 99 | * coupling terms 100 | * [ 101 | * R11 R21 R31 R41 102 | * R12 R22 R32 R42 103 | * R13 R23 R33 R43 104 | * R14 R24 R34 R44 105 | * ] 106 | */ 107 | x_dot[i] += cos(theta[0][i]) * x[0] - sin(theta[0][i]) * y[0]; 108 | y_dot[i] += sin(theta[0][i]) * x[0] + cos(theta[0][i]) * y[0]; 109 | x_dot[i] += cos(theta[1][i]) * x[1] - sin(theta[1][i]) * y[1]; 110 | y_dot[i] += sin(theta[1][i]) * x[1] + cos(theta[1][i]) * y[1]; 111 | x_dot[i] += cos(theta[2][i]) * x[2] - sin(theta[2][i]) * y[2]; 112 | y_dot[i] += sin(theta[2][i]) * x[2] + cos(theta[2][i]) * y[2]; 113 | x_dot[i] += cos(theta[3][i]) * x[3] - sin(theta[3][i]) * y[3]; 114 | y_dot[i] += sin(theta[3][i]) * x[3] + cos(theta[3][i]) * y[3]; 115 | 116 | // update the signal values 117 | x[i] += x_dot[i] * dt; 118 | y[i] += y_dot[i] * dt; 119 | 120 | // Hip Joint Angels 121 | angs[i] = 0 + deg_2_rad(init_angs[i]); 122 | angs[i + 1] = x[i] + deg_2_rad(init_angs[i + 1]); 123 | // Knee Joint Angel 124 | if (y[i] > 0) { 125 | angs[i + 2] = deg_2_rad(init_angs[i + 2]); 126 | } else { 127 | angs[i + 2] = y[i] + deg_2_rad(init_angs[i + 2]); 128 | } 129 | 130 | } 131 | 132 | for (int i = 0; i < 12; i++) { 133 | servos[i]->setPosition(angs[i]); 134 | } 135 | 136 | }; 137 | 138 | // Enter here exit cleanup code. 139 | 140 | delete robot; 141 | return 0; 142 | } 143 | -------------------------------------------------------------------------------- /hardware_test/Quadruped/Quadruped.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(); 5 | const uint8_t SERVO_PWM_FREQ {50}; // 50 Hz 6 | 7 | void setup() { 8 | Serial.begin(9600); 9 | Serial.println("Robot Start!"); 10 | Serial.println("Please select from the menu: "); 11 | Serial.println("1. Robot stand"); 12 | Serial.println("#############################"); 13 | 14 | pwm.begin(); 15 | delay(50); 16 | pwm.setPWMFreq(SERVO_PWM_FREQ); 17 | reset_robot(); 18 | } 19 | 20 | void set_servo_pulse(uint8_t n, double pulse) { 21 | double pulselength; 22 | 23 | pulselength = 1000000; // 1,000,000 us per second 24 | pulselength /= SERVO_PWM_FREQ; 25 | // Serial.print(pulselength); Serial.println(" us per period"); 26 | pulselength /= 4096; // 12 bits of resolution 27 | // Serial.print(pulselength); Serial.println(" us per bit"); 28 | pulse *= 1000; 29 | pulse /= pulselength; 30 | // Serial.println(pulse); 31 | pwm.setPWM(n, 0, pulse); 32 | } 33 | 34 | void write_servo_ang(uint8_t n,uint8_t angle){ 35 | double pulse; 36 | pulse=0.5+angle/90.0; 37 | set_servo_pulse(n,pulse); 38 | } 39 | 40 | 41 | /*** 42 | * Params: 43 | * alpha: Rate of convergence 44 | * mu: control the amplitude of the output signals, Ah = sqrt(mu) 45 | * Ah: Amplitude of the hip joint control signal 46 | * Ak: Amplitude of the knee joint control signal 47 | * beta: Duty cycle of the support phase (Load Factor) 48 | * omega_st / omega_sw = (1 - beta) / beta 49 | * omega_sw: Frequency of the swing phase 50 | * omega_st: Frequency of the support phase 51 | * a: rate of the change between omega_sw and omega_st 52 | * u: (optional, default 0), feedback, EX: u1 <=> x1, u2<=> y2... 53 | * 54 | * Outputs: 55 | * x1 => LF 56 | * x2 => RF 57 | * x3 => LH 58 | * x4 => RH 59 | * 60 | * x, the output of the oscillator, is control signal for hip joint 61 | * the Ascending part is in the swing phase 62 | * the Descending part is in support phase (when the knee joint is frozen) 63 | */ 64 | 65 | /* 66 | const uint16_t SERVOMIN {102}; 67 | const uint16_t SERVOMAX {512}; 68 | uint8_t alpha {100}; 69 | double Ah {0.2}; 70 | double Ak {0.1}; 71 | uint8_t a {10}; 72 | double omega_sw {3 * M_PI}; 73 | 74 | // servo init angle with model coordinate 75 | double servo_int_ang[12] { 76 | 45, 100, 180, 135, 90, 0, 135, 90, 0, 45, 90, 180 77 | }; 78 | int8_t servo_dir[12] { 79 | 1, -1, -1, -1, 1, 1, -1, -1, 1, 1, 1, -1 80 | }; 81 | 82 | // hip0, hip1, knee 83 | double servo_num[12] { 84 | 0, 1, 2, 85 | 3, 4, 5, 86 | 6, 7, 8, 87 | 9, 10, 11 88 | }; 89 | // LF, RF, RH, LF 90 | // walk 0, 0.5, 0.25, 0.75 91 | // trot 0, 0.5, 0, 0.5 92 | double phi[4] {0, 0.5, 0.25, 0.75}; 93 | double beta {0.5}; 94 | 95 | // current angle rad buffer 96 | double current_rad[12] {}; 97 | */ 98 | const uint16_t SERVOMIN {102}; 99 | const uint16_t SERVOMAX {512}; 100 | 101 | // servo init angle with model coordinate 102 | double servo_int_ang[12] { 103 | 45, 100, 180, 135, 90, 0, 135, 90, 0, 45, 90, 180 104 | }; 105 | 106 | int8_t servo_dir[12] { 107 | 1, -1, -1, -1, 1, 1, -1, -1, 1, 1, 1, -1 108 | }; 109 | 110 | // hip0, hip1, knee 111 | double servo_num[12] { 112 | 0, 1, 2, 113 | 3, 4, 5, 114 | 6, 7, 8, 115 | 9, 10, 11 116 | }; 117 | 118 | void reset_robot(){ 119 | pwm.setPWM(0, 0, 0); 120 | pwm.setPWM(1, 0, 0); 121 | pwm.setPWM(2, 0, 0); 122 | pwm.setPWM(3, 0, 0); 123 | pwm.setPWM(4, 0, 0); 124 | pwm.setPWM(5, 0, 0); 125 | pwm.setPWM(6, 0, 0); 126 | } 127 | 128 | // init STAND state 129 | void init_robot() { 130 | Serial.println("init robot"); 131 | 132 | write_servo_ang(0, 45); 133 | write_servo_ang(1, 100); 134 | write_servo_ang(2, 180); 135 | 136 | write_servo_ang(4, 135); 137 | write_servo_ang(5, 90); 138 | write_servo_ang(6, 0); 139 | 140 | // write_servo_ang(8, 135); 141 | // write_servo_ang(9, 90); 142 | // write_servo_ang(10, 0); 143 | } 144 | 145 | String input_str = ""; 146 | void loop() { 147 | if (Serial.available() > 0) { 148 | if (Serial.peek() != '\n') { 149 | input_str += (char)Serial.read(); 150 | } else { 151 | Serial.read(); 152 | Serial.print("Instruction received: "); 153 | Serial.println(input_str); 154 | switch (input_str.toInt()) { 155 | case 1: 156 | init_robot(); 157 | break; 158 | case 2: 159 | reset_robot(); 160 | break; 161 | } 162 | input_str = ""; 163 | } 164 | delay(100); 165 | } 166 | } 167 | -------------------------------------------------------------------------------- /hardware_test/LabRobot/StableGait/QuadrupedRobot.h: -------------------------------------------------------------------------------- 1 | #ifndef QUADRUPED_ROBOT_H 2 | #define QUADRUPED_ROBOT_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #define PT(s) Serial.print(s) 9 | #define PTL(s) Serial.println(s) 10 | #define PTF(s) Serial.print(F(s)) //trade flash memory for dynamic memory with F() function 11 | #define PTLF(s) Serial.println(F(s)) 12 | 13 | //servo constants 14 | #define SERVOMIN 500 15 | #define SERVOMAX 2500 16 | 17 | #define DOF 12 18 | 19 | class QuadrupedRobot 20 | { 21 | private: 22 | // For any non-periodic command, set it to -1 in the end of the action 23 | int command_code{-1}; 24 | Servo *servos_ptr[DOF]{ 25 | nullptr, nullptr, nullptr, 26 | nullptr, nullptr, nullptr, 27 | nullptr, nullptr, nullptr, 28 | nullptr, nullptr, nullptr}; 29 | // HipZ, HipX, Knee 30 | byte servo_pins[DOF]{ 31 | 3, 4, 5, 32 | 6, 7, 8, 33 | 9, 10, 11, 34 | 12, 13, 14}; 35 | // Time taken by each sequence, used in servo_move() 36 | unsigned long timestep = 500; 37 | int steplen = 80; 38 | 39 | /** 40 | * Servo rotation configs 41 | * Real servo coord to calibrated coord: X-roll, Y-yaw, Z-pitch 42 | */ 43 | const float init_servo_deg[12]{35, 90, 180, 125, 83, 5, 40, 85, 180, 127, 83, 0}; 44 | const int8_t servo_dir[12]{1, -1, -1, 1, 1, 1, -1, 1, 1, -1, -1, -1}; 45 | const float toe_out0{30}; // outward distance of toe during stand, in mm 46 | const float dist_ag{30}; // distance between alfa and gamma axis, in mm 47 | const float thigh{107}; 48 | const float calf{90}; 49 | /** 50 | * Body configs 51 | */ 52 | const float feet_dist_z{70}; // distance between feet in z axis, in mm 53 | const float feet_dist_x{220}; // distance between feet in x axis, in mm 54 | // distance from center to the foot 55 | const float body_radius{sqrt(pow(feet_dist_z / 2, 2) + pow(feet_dist_x / 2, 2))}; 56 | const float body_phi0{atan(feet_dist_z / feet_dist_x)}; 57 | /** 58 | * vleg_len: virtual leg length (composite with 2 limbs) 59 | * alfa, gamma, beta is conresponding to angles on HipZ, Knee, HipX axises 60 | */ 61 | const float vleg_len0{160}; 62 | const float height0{sqrt(pow(vleg_len0 + dist_ag, 2) - pow(toe_out0, 2))}; 63 | const float alfa0{acos((thigh * thigh + vleg_len0 * vleg_len0 - calf * calf) / (2 * thigh * vleg_len0))}; 64 | const float beta0{M_PI - acos((thigh * thigh + calf * calf - vleg_len0 * vleg_len0) / (2 * thigh * calf))}; 65 | const float gamma0{asin(toe_out0 / (vleg_len0 + dist_ag))}; 66 | /** 67 | * State Variables 68 | * angles in radiums 69 | * Foot position, order: FLxyz, FRxyz, RLxyz, RRxyz 70 | */ 71 | float joint_angs_pre[12]{}; //Previous joint angles 72 | float joint_angs_new[12]{}; 73 | float vlegs_len[4]{}; 74 | float foot_pos[12] {}; 75 | float step_turn[12] {}; // foot step in case of turn 76 | /** 77 | * Configed States 78 | * Angles in callibrated coordinate 79 | */ 80 | const float stand_angs[12]{0, -30, 60, 0, -30, 60, 0, 30, -60, 0, 30, -60}; 81 | const float rest_angs[12]{0, -55, 130, 0, -55, 130, 0, 55, -130, 0, 55, -130}; 82 | const float adjust_angs[12]{}; 83 | const float turn_phi{20}; 84 | // 85 | // X points to forward, Z points to upward 86 | 87 | 88 | void setup_servos(); 89 | void shut_servos(); 90 | void servo_move(float *angs_ptr); 91 | void servo_write_angs(float *angs_ptr, bool is_in_degrees); 92 | void clear_cmd(); 93 | 94 | void body_xyz(float x, float y, float z); 95 | void body_move_xyz(float dx, float dy, float dz); 96 | void body_turn_left(); 97 | void body_turn_right(); 98 | void foot_step(int i, float x, float z); 99 | void foot_step_ang(int i); 100 | void foot_move_xyz(int i, float dx, float dy, float dz); 101 | void turn_pose(float ang); 102 | 103 | void inverse_kinematics(); 104 | float gamma_left(float dy, float dz); 105 | float gamma_right(float dy, float dz); 106 | float vleg_left(float dx, float dy, float gamma); 107 | float vleg_right(float dx, float dy, float gamma); 108 | float beta_front(float vleg_len); 109 | float beta_rear(float vleg_len); 110 | float alfa_front(float dx, float beta, float vleg_len); 111 | float alfa_rear(float dx, float beta, float vleg_len); 112 | 113 | void pause(); 114 | 115 | public: 116 | ~QuadrupedRobot(); 117 | bool interrupt(); 118 | void parse_code_run(); 119 | 120 | void switch_on(); 121 | void switch_off(); 122 | 123 | void adjust(); 124 | void bot_rest(); 125 | void bot_stand(); 126 | void bot_walk(); 127 | void bot_turn_right(); 128 | void bot_turn_left(); 129 | }; 130 | 131 | #endif 132 | -------------------------------------------------------------------------------- /webots_sim/controllers/quadruped_controller/quadruped_controller.py: -------------------------------------------------------------------------------- 1 | """quadruped_controller controller.""" 2 | 3 | from controller import Robot 4 | import math 5 | import numpy as np 6 | 7 | 8 | def setAngles(motors, angles, posSensors): 9 | # 0 16 36 0 35 83 0 -13 9 0 22 9 10 | direction = [1, -1, 1, 1, -1, 1, 1, 1, -1, 1, 1, -1] 11 | theta = [] 12 | for i in range(12): 13 | theta.append(angles[i] * direction[i] * math.pi / 180) 14 | setPositions(motors, theta, posSensors) 15 | 16 | 17 | def setPositions(motors, theta, posSensors): 18 | """ 19 | set joint positions 20 | not feedback control 21 | """ 22 | delta = 0.01 23 | for i in range(len(motors)): 24 | motors[i].setPosition(theta[i]) 25 | 26 | # while robot.step(timestep) != -1: 27 | # checked = True 28 | # for i in range(len(motors)): 29 | # effective = posSensors[i].getValue() 30 | # target = theta[i] 31 | 32 | # if math.fabs(effective - target) > delta: 33 | # print('i th part: ', i) 34 | # print('error: ', effective - target) 35 | # checked = False 36 | # if checked: 37 | # break 38 | 39 | 40 | def setVelocity(motors, v): 41 | """ 42 | v: rad/s 43 | """ 44 | for i in range(len(motors)): 45 | motors[i].setVelocity(v) 46 | 47 | 48 | theta_list = [ 49 | [0,16,36], 50 | [0,17,36], 51 | [0,17,36], 52 | [0,18,36], 53 | [0,19,36], 54 | [0,19,35], 55 | [0,20,35], 56 | [0,20,35], 57 | [0,21,34], 58 | [0,22,34], 59 | [0,22,33], 60 | [0,22,32], 61 | [0,23,31], 62 | [0,23,31], 63 | [0,23,30], 64 | [0,24,29], 65 | [0,24,27], 66 | [0,24,26], 67 | [0,24,25], 68 | [0,24,23], 69 | [0,24,21], 70 | [0,24,19], 71 | [0,23,17], 72 | [0,23,14], 73 | [0,22,9], 74 | [0,22,9], 75 | [0,27,22], 76 | [0,30,30], 77 | [0,33,36], 78 | [0,35,41], 79 | [0,37,46], 80 | [0,38,50], 81 | [0,39,54], 82 | [0,40,57], 83 | [0,41,60], 84 | [0,42,63], 85 | [0,42,66], 86 | [0,43,68], 87 | [0,43,70], 88 | [0,43,73], 89 | [0,43,74], 90 | [0,43,76], 91 | [0,42,78], 92 | [0,42,79], 93 | [0,41,80], 94 | [0,41,81], 95 | [0,40,82], 96 | [0,39,82], 97 | [0,38,83], 98 | [0,37,83], 99 | [0,35,83], 100 | [0,34,83], 101 | [0,33,82], 102 | [0,31,82], 103 | [0,30,81], 104 | [0,29,80], 105 | [0,27,79], 106 | [0,25,78], 107 | [0,24,76], 108 | [0,22,74], 109 | [0,21,73], 110 | [0,19,70], 111 | [0,17,68], 112 | [0,15,66], 113 | [0,14,63], 114 | [0,12,60], 115 | [0,10,57], 116 | [0,8,54], 117 | [0,6,50], 118 | [0,4,46], 119 | [0,1,41], 120 | [0,0,36], 121 | [0,-3,30], 122 | [0,-7,22], 123 | [0,-13,9], 124 | [0,-13,9], 125 | [0,-10,14], 126 | [0,-8,17], 127 | [0,-6,19], 128 | [0,-5,21], 129 | [0,-3,23], 130 | [0,-2,25], 131 | [0,0,26], 132 | [0,0,27], 133 | [0,1,29], 134 | [0,3,30], 135 | [0,4,31], 136 | [0,5,31], 137 | [0,6,32], 138 | [0,7,33], 139 | [0,8,34], 140 | [0,9,34], 141 | [0,10,35], 142 | [0,11,35], 143 | [0,12,35], 144 | [0,13,36], 145 | [0,14,36], 146 | [0,14,36], 147 | [0,15,36], 148 | [0,16,36] 149 | ] 150 | 151 | theta_list_reversed = list(reversed(theta_list)) 152 | 153 | # create the Robot instance. 154 | robot = Robot() 155 | 156 | # get the time step of the current world. 157 | timestep = int(robot.getBasicTimeStep()) 158 | 159 | # init 12 DOF motors and position sensors 160 | motorNames = [ 161 | 'hip0x', 'hip0z', 'knee0', 162 | 'hip1x', 'hip1z', 'knee1', 163 | 'hip2x', 'hip2z', 'knee2', 164 | 'hip3x', 'hip3z', 'knee3' 165 | ] 166 | motors = [ 167 | robot.getMotor(motorNames[i]) for i in range(len(motorNames)) 168 | ] 169 | posSensors = [ 170 | robot.getPositionSensor(motorNames[i] + '_pos') for i in range(len(motorNames)) 171 | ] 172 | for posSensor in posSensors: 173 | posSensor.enable(timestep) 174 | 175 | # init positions 176 | # thetaInit = np.array([ 177 | # 0, -45/180 * math.pi, 90/180 * math.pi, 178 | # 0, -45/180 * math.pi, 90/180 * math.pi, 179 | # 0, 45/180 * math.pi, -90/180 * math.pi, 180 | # 0, 45/180 * math.pi, -90/180 * math.pi 181 | # ]) 182 | 183 | # setPositions(motors, thetaInit, posSensors) 184 | setVelocity(motors, 10) 185 | 186 | theta_list_len = len(theta_list) 187 | lf_index = 0 188 | 189 | # Main loop: 190 | # - perform simulation steps until Webots is stopping the controller 191 | while robot.step(timestep) != -1: 192 | rf_index = (lf_index + theta_list_len // 2) % theta_list_len 193 | lr_index = (lf_index - theta_list_len // 4) % theta_list_len 194 | rr_index = (lf_index - 3 * theta_list_len // 4) % theta_list_len 195 | lf_angles = np.array(theta_list[lf_index]) 196 | rf_angles = np.array(theta_list[rf_index]) 197 | lr_angles = np.array(theta_list_reversed[lr_index]) 198 | rr_angles = np.array(theta_list_reversed[rr_index]) 199 | op_angles = np.concatenate((rf_angles, lf_angles, rr_angles, lr_angles)) 200 | setAngles(motors, op_angles, posSensors) 201 | 202 | lf_index = (lf_index + 1) % theta_list_len 203 | 204 | pass 205 | -------------------------------------------------------------------------------- /mathematics/walk_trot_transition/code.py: -------------------------------------------------------------------------------- 1 | ### 2 | # Quadruped Robot gait transitions between walk and trot 3 | # CPG controller is based on 4 HOPF Oscillators (Read cpg fold to known more) 4 | # 5 | # For Walk: 6 | # beta = 0.75 7 | # phi_LF = 0 8 | # phi_RF = 0.5 9 | # phi_LH = 0.75 10 | # phi_RH = 0.25 11 | # 12 | # For Trot: 13 | # beta = 0.5 14 | # phi_LF = 0 15 | # phi_RF = 0.5 16 | # phi_LH = 0 17 | # phi_RH = 0.5 18 | # 19 | # Then: 20 | # Phi3 = 0.25 or 0 21 | # Phi1 = 0, Phi2 = 0.5, Phi4 = 0.5 + Phi3 22 | # Beta = 0.5 + Phi3 23 | # theta21 = - pi 24 | # theta31 = - Phi3 * 2 * pi 25 | # theta41 = - (0.5 + Phi3) * 2 * pi 26 | # theta12 = pi 27 | # theta32 = (0.5 - Phi3) * 2 * pi 28 | # theta42 = - Phi3 * 2 * pi 29 | # theta13 = Phi3 * 2 * pi 30 | # theta23 = (Phi3 - 0.5) * 2 * pi 31 | # theta43 = - pi 32 | # theta14 = (0.5 + Phi3) * 2 * pi 33 | # theta24 = Phi3 * 2 * pi 34 | # theta34 = pi 35 | # 36 | ### 37 | 38 | import matplotlib.pyplot as plt 39 | import numpy as np 40 | 41 | alpha = 1000 42 | Ah = 1 43 | Ak = 0.5 44 | omega_sw = 5 * np.pi 45 | a = 10 46 | mu = Ah ** 2 47 | 48 | phi3 = 0.25 49 | 50 | # phase config 51 | phi = [0, 0.5, phi3, 0.5 + phi3] 52 | beta = 0.5 + phi3 53 | omega_st = omega_sw * (1 - beta) / beta 54 | 55 | # 5 seconds 56 | sim_duration = 10 57 | dt = 0.01 58 | iteration = 100 59 | 60 | x1, y1, x2, y2, x3, y3, x4, y4 = (1, 0, 0, 0, 0, 0, 0, 0) 61 | Q1 = np.matrix([x1, y1]).T 62 | Q2 = np.matrix([x2, y2]).T 63 | Q3 = np.matrix([x3, y3]).T 64 | Q4 = np.matrix([x4, y4]).T 65 | Q = np.vstack([Q1, Q2, Q3, Q4]) 66 | 67 | # result collection 68 | theta_h1_t = [] 69 | theta_h2_t = [] 70 | theta_h3_t = [] 71 | theta_h4_t = [] 72 | theta_k1_t = [] 73 | theta_k2_t = [] 74 | theta_k3_t = [] 75 | theta_k4_t = [] 76 | 77 | t_t = np.arange(0, sim_duration, dt) 78 | 79 | for t in t_t: 80 | for _ in np.arange(iteration): 81 | # change the phi3 to 0 from 2s 82 | if t >= 4 and phi3 > 0: 83 | phi3 -= 0.01 84 | phi = [0, 0.5, phi3, 0.5 + phi3] 85 | beta = 0.5 + phi3 86 | omega_st = omega_sw * (1 - beta) / beta 87 | 88 | r1_square = x1 ** 2 + y1 ** 2 89 | r2_square = x2 ** 2 + y2 ** 2 90 | r3_square = x3 ** 2 + y3 ** 2 91 | r4_square = x4 ** 2 + y4 ** 2 92 | 93 | # Frequency of the Oscillator 94 | omega1 = omega_st / (np.exp(- a * y1) + 1) + omega_sw / (np.exp(a * y1) + 1) 95 | omega2 = omega_st / (np.exp(- a * y2) + 1) + omega_sw / (np.exp(a * y2) + 1) 96 | omega3 = omega_st / (np.exp(- a * y3) + 1) + omega_sw / (np.exp(a * y3) + 1) 97 | omega4 = omega_st / (np.exp(- a * y4) + 1) + omega_sw / (np.exp(a * y4) + 1) 98 | 99 | FQ = np.matrix([ 100 | alpha * (mu - r1_square) * x1 - omega1 * y1, 101 | alpha * (mu - r1_square) * y1 + omega1 * x1, 102 | alpha * (mu - r2_square) * x2 - omega2 * y2, 103 | alpha * (mu - r2_square) * y2 + omega2 * x2, 104 | alpha * (mu - r3_square) * x3 - omega3 * y3, 105 | alpha * (mu - r3_square) * y3 + omega3 * x3, 106 | alpha * (mu - r4_square) * x4 - omega4 * y4, 107 | alpha * (mu - r4_square) * y4 + omega4 * x4 108 | ]).T 109 | 110 | """ 111 | Coupling Matrix of 4 Oscillators 112 | R = [ 113 | R11, R21, R31, R41; 114 | R12, R22, R32, R42; 115 | R13, R23, R33, R43; 116 | R14, R24, R34, R44 117 | ] 118 | 119 | Rji = [ 120 | cos(theta_ji), -sin(theta_ji); 121 | sin(theta_ji), cos(theta_ji) 122 | ] 123 | 124 | theta_ji = phi_j - phi_i 125 | """ 126 | R = np.asmatrix(np.full((8, 8), None)) 127 | for i in range(0, 4): 128 | for j in range(0, 4): 129 | theta_ji = 2 * np.pi * (phi[j] - phi[i]) 130 | R[i * 2, j * 2] = np.cos(theta_ji) 131 | R[i * 2, j * 2 + 1] = - np.sin(theta_ji) 132 | R[i * 2 + 1, j * 2] = np.sin(theta_ji) 133 | R[i * 2 + 1, j * 2 + 1] = R[i * 2, j * 2] 134 | 135 | # Q_dot = F(Q) + RQ 136 | Q_dot = FQ + np.dot(R, Q) 137 | 138 | Q = Q + Q_dot * dt / iteration 139 | x1 = Q[0, 0] 140 | y1 = Q[1, 0] 141 | x2 = Q[2, 0] 142 | y2 = Q[3, 0] 143 | x3 = Q[4, 0] 144 | y3 = Q[5, 0] 145 | x4 = Q[6, 0] 146 | y4 = Q[7, 0] 147 | 148 | # Signal Data Collection 149 | theta_h1_t.append(x1) 150 | theta_h2_t.append(x2) 151 | theta_h3_t.append(x3) 152 | theta_h4_t.append(x4) 153 | # 154 | # if y1 > 0: 155 | # theta_k1 = 0 156 | # else: 157 | # theta_k1 = - Ak / Ah * y1 158 | # 159 | # if y2 > 0: 160 | # theta_k2 = 0 161 | # else: 162 | # theta_k2 = - Ak / Ah * y2 163 | # 164 | # if y3 > 0: 165 | # theta_k3 = 0 166 | # else: 167 | # theta_k3 = Ak / Ah * y3 168 | # 169 | # if y4 > 0: 170 | # theta_k4 = 0 171 | # else: 172 | # theta_k4 = Ak / Ah * y4 173 | # 174 | # theta_k1_t.append(theta_k1) 175 | # theta_k2_t.append(theta_k2) 176 | # theta_k3_t.append(theta_k3) 177 | # theta_k4_t.append(theta_k4) 178 | 179 | plt.figure() 180 | plt.subplot() 181 | plt.plot(t_t, theta_h1_t, '-', label='limb1') 182 | plt.plot(t_t, theta_h2_t, '--', label='limb2') 183 | plt.plot(t_t, theta_h3_t, '-.', label='limb3') 184 | plt.plot(t_t, theta_h4_t, ':', label='limb4') 185 | plt.ylabel('joint angle/rad') 186 | plt.xlabel('time/s') 187 | plt.grid() 188 | plt.legend() 189 | plt.show() 190 | -------------------------------------------------------------------------------- /mathematics/cpg/code.py: -------------------------------------------------------------------------------- 1 | ### 2 | # CPG controller based on 4 Hopf Oscillators 3 | # 4 | # Params: 5 | # alpha: Rate of convergence 6 | # mu: control the amplitude of the output signals, Ah = sqrt(mu) 7 | # Ah: Amplitude of the hip joint control signal 8 | # Ak: Amplitude of the knee joint control signal 9 | # beta: Duty cycle of the support phase (Duty Factor) 10 | # omega_st / omega_sw = (1 - beta) / beta 11 | # omega_sw: Frequency of the swing phase 12 | # omega_st: Frequency of the support phase 13 | # a: rate of the change between omega_sw and omega_st 14 | # u: (optional, default 0), feedback, EX: u1 <=> x1, u2<=> y2... 15 | # 16 | # Outputs: 17 | # x1 => LF 18 | # x2 => RF 19 | # x3 => LH 20 | # x4 => RH 21 | # 22 | # x, the output of the oscillator, is control signal for hip joint 23 | # the Ascending part is in the swing phase 24 | # the Descending part is in support phase (when the knee joint is frozen) 25 | ### 26 | 27 | import matplotlib.pyplot as plt 28 | import numpy as np 29 | 30 | alpha = 10000 31 | beta = 0.75 32 | Ah = 1 33 | Ak = 0.5 34 | omega_sw = 5 * np.pi 35 | omega_st = omega_sw * (1 - beta) / beta 36 | a = 10 37 | mu = Ah ** 2 38 | 39 | # phase config of walk gait 40 | phi_LF = 0 41 | phi_RF = 0.5 42 | phi_LH = 0.75 43 | phi_RH = 0.25 44 | # phase order 45 | phi = [phi_LF, phi_RF, phi_RH, phi_LH] 46 | 47 | # 5 seconds 48 | sim_duration = 5 49 | dt = 0.01 50 | iteration = 100 51 | 52 | x1, y1, x2, y2, x3, y3, x4, y4 = (1, 0, 0, 0, 0, 0, 0, 0) 53 | Q1 = np.matrix([x1, y1]).T 54 | Q2 = np.matrix([x2, y2]).T 55 | Q3 = np.matrix([x3, y3]).T 56 | Q4 = np.matrix([x4, y4]).T 57 | Q = np.vstack([Q1, Q2, Q3, Q4]) 58 | 59 | # result collection 60 | theta_h1_t = [] 61 | theta_h2_t = [] 62 | theta_h3_t = [] 63 | theta_h4_t = [] 64 | theta_k1_t = [] 65 | theta_k2_t = [] 66 | theta_k3_t = [] 67 | theta_k4_t = [] 68 | 69 | t_t = np.arange(0, sim_duration, dt) 70 | 71 | for t in t_t: 72 | for _ in np.arange(iteration): 73 | r1_square = x1 ** 2 + y1 ** 2 74 | r2_square = x2 ** 2 + y2 ** 2 75 | r3_square = x3 ** 2 + y3 ** 2 76 | r4_square = x4 ** 2 + y4 ** 2 77 | 78 | # Frequency of the Oscillator 79 | omega1 = omega_st / (np.exp(- a * y1) + 1) + omega_sw / (np.exp(a * y1) + 1) 80 | omega2 = omega_st / (np.exp(- a * y2) + 1) + omega_sw / (np.exp(a * y2) + 1) 81 | omega3 = omega_st / (np.exp(- a * y3) + 1) + omega_sw / (np.exp(a * y3) + 1) 82 | omega4 = omega_st / (np.exp(- a * y4) + 1) + omega_sw / (np.exp(a * y4) + 1) 83 | 84 | FQ = np.matrix([ 85 | alpha * (mu - r1_square) * x1 - omega1 * y1, 86 | alpha * (mu - r1_square) * y1 + omega1 * x1, 87 | alpha * (mu - r2_square) * x2 - omega2 * y2, 88 | alpha * (mu - r2_square) * y2 + omega2 * x2, 89 | alpha * (mu - r3_square) * x3 - omega3 * y3, 90 | alpha * (mu - r3_square) * y3 + omega3 * x3, 91 | alpha * (mu - r4_square) * x4 - omega4 * y4, 92 | alpha * (mu - r4_square) * y4 + omega4 * x4 93 | ]).T 94 | 95 | """ 96 | Coupling Matrix of 4 Oscillators 97 | R = [ 98 | R11, R21, R31, R41; 99 | R12, R22, R32, R42; 100 | R13, R23, R33, R43; 101 | R14, R24, R34, R44 102 | ] 103 | 104 | Rji = [ 105 | cos(theta_ji), -sin(theta_ji); 106 | sin(theta_ji), cos(theta_ji) 107 | ] 108 | 109 | theta_ji = phi_j - phi_i 110 | """ 111 | R = np.asmatrix(np.full((8, 8), None)) 112 | for i in range(0, 4): 113 | for j in range(0, 4): 114 | theta_ji = 2 * np.pi * (phi[i] - phi[j]) 115 | R[i * 2, j * 2] = np.cos(theta_ji) 116 | R[i * 2, j * 2 + 1] = - np.sin(theta_ji) 117 | R[i * 2 + 1, j * 2] = np.sin(theta_ji) 118 | R[i * 2 + 1, j * 2 + 1] = R[i * 2, j * 2] 119 | 120 | # Q_dot = F(Q) + RQ 121 | Q_dot = FQ + np.dot(R, Q) 122 | 123 | Q = Q + Q_dot * dt / iteration 124 | x1 = Q[0, 0] 125 | y1 = Q[1, 0] 126 | x2 = Q[2, 0] 127 | y2 = Q[3, 0] 128 | x3 = Q[4, 0] 129 | y3 = Q[5, 0] 130 | x4 = Q[6, 0] 131 | y4 = Q[7, 0] 132 | 133 | # Signal Data Collection 134 | theta_h1_t.append(x1) 135 | theta_h2_t.append(x2) 136 | theta_h3_t.append(x3) 137 | theta_h4_t.append(x4) 138 | 139 | if y1 > 0: 140 | theta_k1 = 0 141 | else: 142 | theta_k1 = - Ak / Ah * y1 143 | 144 | if y2 > 0: 145 | theta_k2 = 0 146 | else: 147 | theta_k2 = - Ak / Ah * y2 148 | 149 | if y3 > 0: 150 | theta_k3 = 0 151 | else: 152 | theta_k3 = Ak / Ah * y3 153 | 154 | if y4 > 0: 155 | theta_k4 = 0 156 | else: 157 | theta_k4 = Ak / Ah * y4 158 | 159 | theta_k1_t.append(theta_k1) 160 | theta_k2_t.append(theta_k2) 161 | theta_k3_t.append(theta_k3) 162 | theta_k4_t.append(theta_k4) 163 | 164 | plt.figure() 165 | plt.subplot(411) 166 | plt.plot(t_t, theta_h1_t, label='hip') 167 | plt.plot(t_t, theta_k1_t, label='knee') 168 | plt.ylabel('LF') 169 | plt.grid() 170 | 171 | plt.subplot(412) 172 | plt.plot(t_t, theta_h2_t, label='hip') 173 | plt.plot(t_t, theta_k2_t, label='knee') 174 | plt.ylabel('RF') 175 | plt.grid() 176 | 177 | plt.subplot(413) 178 | plt.plot(t_t, theta_h3_t, label='hip') 179 | plt.plot(t_t, theta_k3_t, label='knee') 180 | plt.ylabel('RH') 181 | plt.grid() 182 | 183 | plt.subplot(414) 184 | plt.plot(t_t, theta_h4_t, label='hip') 185 | plt.plot(t_t, theta_k4_t, label='knee') 186 | plt.ylabel('LH') 187 | plt.grid() 188 | 189 | plt.subplots_adjust(top=0.92, bottom=0.2, hspace=0.5, wspace=0.35) 190 | plt.legend() 191 | 192 | 193 | plt.show() 194 | -------------------------------------------------------------------------------- /hardware_test/LabRobot/DogRobot/QuadrupedRobot.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "QuadrupedRobot.h" 4 | 5 | QuadrupedRobot::QuadrupedRobot() 6 | { 7 | } 8 | 9 | QuadrupedRobot::~QuadrupedRobot() 10 | { 11 | } 12 | 13 | void QuadrupedRobot::setup_servos() 14 | { 15 | PTL("Setup Servos"); 16 | for (size_t i = 0; i < DOF; i++) 17 | { 18 | if (servos[i] == nullptr) 19 | servos[i] = new Servo(); 20 | servos[i]->attach(pins[i], SERVOMIN, SERVOMAX); 21 | } 22 | } 23 | 24 | void QuadrupedRobot::shut_servos() 25 | { 26 | for (size_t i = 0; i < DOF; i++) 27 | { 28 | if (servos[i] != nullptr) 29 | { 30 | servos[i]->detach(); 31 | } 32 | } 33 | } 34 | 35 | /** 36 | * Servo Write Current Angles 37 | * Current Angles is in Callibrated Coordinate, 38 | * which should be transfered to real angle degrees before using. 39 | */ 40 | void QuadrupedRobot::servo_write_angs() 41 | { 42 | // PTL("Servo Write Angles"); 43 | double real_ang{}; 44 | double pulsewidth{}; 45 | for (size_t i = 0; i < DOF; i++) 46 | { 47 | real_ang = init_angs[i] + dir[i] * current_angs_ptr[i]; 48 | pulsewidth = 500 + real_ang / 90.0 * 1000; 49 | if (!servos[i]->attached()) 50 | { 51 | servos[i]->attach(pins[i], SERVOMIN, SERVOMAX); 52 | } 53 | servos[i]->writeMicroseconds(pulsewidth); 54 | } 55 | } 56 | 57 | /** 58 | * CPG (Central Pattern Generator) 59 | * Based on HOPF oscillator 60 | * Output: current_angs_ptr will be updated with output angles. 61 | */ 62 | void QuadrupedRobot::cpg_signal() 63 | { 64 | double *pose_angs_ptr{stand_angs}; 65 | // -1 for elbow style, 1 for knee style 66 | int8_t knee_factor[4]{-1, -1, 1, 1}; 67 | // dt = 0.0001s, dt * 100 = 0.01s 68 | for (size_t itr = 0; itr < CPG_ITERATES; itr++) { 69 | for (byte i = 0; i < 4; i++) { 70 | r_square[i] = x[i] * x[i] + y[i] * y[i]; // 1 71 | 72 | // Frequency of the oscillator 73 | omega[i] = omega_st / (exp(- a * y[i]) + 1) + omega_sw / (exp(a * y[i]) + 1); // 9.42 74 | 75 | // HOPF Oscillator 76 | x_dot[i] = alpha * (mu - r_square[i]) * x[i] - omega[i] * y[i]; 77 | y_dot[i] = alpha * (mu - r_square[i]) * y[i] + omega[i] * x[i]; 78 | 79 | /** 80 | * coupling terms 81 | * [ 82 | * R11 R21 R31 R41 83 | * R12 R22 R32 R42 84 | * R13 R23 R33 R43 85 | * R14 R24 R34 R44 86 | * ] 87 | */ 88 | x_dot[i] += cos(theta[0][i]) * x[0] - sin(theta[0][i]) * y[0]; 89 | y_dot[i] += sin(theta[0][i]) * x[0] + cos(theta[0][i]) * y[0]; 90 | x_dot[i] += cos(theta[1][i]) * x[1] - sin(theta[1][i]) * y[1]; 91 | y_dot[i] += sin(theta[1][i]) * x[1] + cos(theta[1][i]) * y[1]; 92 | x_dot[i] += cos(theta[2][i]) * x[2] - sin(theta[2][i]) * y[2]; 93 | y_dot[i] += sin(theta[2][i]) * x[2] + cos(theta[2][i]) * y[2]; 94 | x_dot[i] += cos(theta[3][i]) * x[3] - sin(theta[3][i]) * y[3]; 95 | y_dot[i] += sin(theta[3][i]) * x[3] + cos(theta[3][i]) * y[3]; 96 | 97 | // update the signal values 98 | x[i] += x_dot[i] * CPG_DT; 99 | y[i] += y_dot[i] * CPG_DT; 100 | 101 | // Hip Joint Angels 102 | current_angs_ptr[i * 3] = 0 + pose_angs_ptr[i * 3]; 103 | current_angs_ptr[i * 3 + 1] = x[i] * 180 / M_PI + pose_angs_ptr[i * 3 + 1]; 104 | 105 | // Knee Joint Angel 106 | if (y[i] > 0) { 107 | current_angs_ptr[i * 3 + 2] = pose_angs_ptr[i * 3 + 2]; 108 | } else { 109 | current_angs_ptr[i * 3 + 2] = y[i] * knee_factor[i] * Ak * 180 / (M_PI * Ah) + pose_angs_ptr[i * 3 + 2]; 110 | } 111 | } 112 | } 113 | } 114 | 115 | void QuadrupedRobot::switch_on() 116 | { 117 | if(lock) return; 118 | PTL("Robot Switch On"); 119 | setup_servos(); 120 | bot_rest(); 121 | delay(1000); 122 | shut_servos(); 123 | } 124 | 125 | void QuadrupedRobot::switch_off() 126 | { 127 | if(lock) return; 128 | PTL("Robot Switch Off"); 129 | shut_servos(); 130 | } 131 | 132 | void QuadrupedRobot::adjust() 133 | { 134 | if(lock) return; 135 | double *temp {current_angs_ptr}; 136 | double adjust_angs[12] {}; 137 | current_angs_ptr = adjust_angs; 138 | servo_write_angs(); 139 | current_angs_ptr = temp; 140 | } 141 | 142 | void QuadrupedRobot::bot_rest() 143 | { 144 | if(lock) return; 145 | PTL("Robot Rest"); 146 | current_angs_ptr = rest_angs; 147 | servo_write_angs(); 148 | } 149 | 150 | void QuadrupedRobot::bot_stand() 151 | { 152 | if(lock) return; 153 | PTL("Robot Stand"); 154 | current_angs_ptr = stand_angs; 155 | servo_write_angs(); 156 | } 157 | 158 | /** 159 | * Walk 160 | * T = 4 * PI / omega_sw 161 | */ 162 | void QuadrupedRobot::bot_walk() 163 | { 164 | if(lock) return; 165 | // LF, RF, LH, RH 166 | double phi[4]{0, 0.5, 0.25, 0.75}; 167 | // init data for CPG 168 | beta = 0.75; 169 | Ah = 0.2; 170 | omega_sw = 2 * M_PI; 171 | omega_st = omega_sw * (1 - beta) / beta; 172 | mu = Ah * Ah; 173 | for (int i = 0; i < 4; i++) { 174 | for (int j = 0; j < 4; j++) { 175 | theta[i][j] = 2 * M_PI * (phi[i] - phi[j]); 176 | } 177 | } 178 | current_angs_ptr = new double [DOF]; 179 | cpg_signal(); 180 | servo_write_angs(); 181 | delete [] current_angs_ptr; 182 | } 183 | 184 | /** 185 | * Trot 186 | * T = 3 * PI / omega_sw 187 | */ 188 | void QuadrupedRobot::bot_trot() 189 | { 190 | if(lock) return; 191 | // LF, RF, LH, RH 192 | double phi[4]{0, 0.5, 0, 0.5}; 193 | // init data for CPG 194 | beta = 0.5; 195 | omega_sw = 5 * M_PI; 196 | omega_st = omega_sw * (1 - beta) / beta; 197 | mu = Ah * Ah; 198 | for (int i = 0; i < 4; i++) { 199 | for (int j = 0; j < 4; j++) { 200 | theta[i][j] = 2 * M_PI * (phi[i] - phi[j]); 201 | } 202 | } 203 | current_angs_ptr = new double[DOF]; 204 | cpg_signal(); 205 | servo_write_angs(); 206 | delete [] current_angs_ptr; 207 | } 208 | -------------------------------------------------------------------------------- /hardware_test/LabRobot/StableGait/QuadrupedRobot.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include "QuadrupedRobot.h" 5 | 6 | /** 7 | * Switch on the robot 8 | */ 9 | void QuadrupedRobot::switch_on() 10 | { 11 | Serial.println("Robot Switch On"); 12 | setup_servos(); 13 | bot_rest(); 14 | delay(2000); 15 | shut_servos(); 16 | clear_cmd(); 17 | } 18 | 19 | /** 20 | * Switch off the robot 21 | */ 22 | void QuadrupedRobot::switch_off() 23 | { 24 | Serial.println("Robot Switch Off"); 25 | shut_servos(); 26 | clear_cmd(); 27 | } 28 | 29 | void QuadrupedRobot::clear_cmd() 30 | { 31 | command_code = -1; 32 | } 33 | 34 | void QuadrupedRobot::setup_servos() 35 | { 36 | for (size_t i = 0; i < DOF; i++) 37 | { 38 | if (servos_ptr[i] == nullptr) 39 | servos_ptr[i] = new Servo(); 40 | servos_ptr[i]->attach(servo_pins[i], SERVOMIN, SERVOMAX); 41 | } 42 | } 43 | 44 | void QuadrupedRobot::shut_servos() 45 | { 46 | for (size_t i = 0; i < DOF; i++) 47 | { 48 | if (servos_ptr[i] != nullptr) 49 | { 50 | servos_ptr[i]->detach(); 51 | } 52 | } 53 | } 54 | 55 | /** 56 | * Check Interrupts 57 | * New Command from Serial Port 58 | */ 59 | bool QuadrupedRobot::interrupt() 60 | { 61 | String serial_in_str {""}; 62 | while(Serial.available() > 0) { 63 | char inChar = Serial.read(); 64 | serial_in_str += (char)inChar; 65 | // wait for input buffer read 66 | delay(10); 67 | } 68 | if(serial_in_str != "") 69 | { 70 | Serial.print("Command received: "); 71 | Serial.println(serial_in_str); 72 | command_code = serial_in_str.toInt(); 73 | return true; 74 | } 75 | return false; 76 | } 77 | 78 | void QuadrupedRobot::parse_code_run() 79 | { 80 | switch(command_code) { 81 | case 1: 82 | switch_on(); 83 | break; 84 | case 2: 85 | bot_stand(); 86 | break; 87 | case 3: 88 | bot_walk(); 89 | break; 90 | case 4: 91 | bot_turn_left(); 92 | break; 93 | case 5: 94 | bot_turn_right(); 95 | break; 96 | case 0: 97 | switch_off(); 98 | break; 99 | case 998: 100 | adjust(); 101 | break; 102 | default: 103 | ;// doing nothing 104 | } 105 | } 106 | 107 | /** 108 | * Synchronous Servo Move 109 | * Every servo should move to the desired position at the end of the timestep 110 | */ 111 | void QuadrupedRobot::servo_move(float *angs_ptr) 112 | { 113 | unsigned long starttime = millis(); 114 | unsigned long timenow = starttime; 115 | unsigned long servo_time[DOF] {}; // Timestamp of servo 116 | float joint_angs_now[DOF] {}; 117 | float joint_degs_speed[DOF] {}; 118 | for (size_t i = 0; i < DOF; i++) 119 | { 120 | joint_degs_speed[i] = (angs_ptr[i] - joint_angs_pre[i]) / timestep; 121 | joint_angs_now[i] = joint_angs_pre[i]; // Reset Joint angles 122 | servo_time[i] = starttime; // Reset Servo Time 123 | } 124 | while((timenow - starttime) < timestep) { 125 | for (size_t i = 0; i < DOF; i++) 126 | { 127 | timenow = millis(); 128 | joint_angs_now[i] += joint_degs_speed[i] * (timenow - servo_time[i]); 129 | servo_time[i] = timenow; // update servo time 130 | } 131 | servo_write_angs(joint_angs_now, false); 132 | timenow = millis(); 133 | } 134 | // last run to assure that all servo has been desired position 135 | for (size_t i = 0; i < DOF; i++) 136 | { 137 | servo_write_angs(angs_ptr, false); 138 | } 139 | } 140 | 141 | /** 142 | * Servo Write Current Angles 143 | * Current Angles is in Callibrated Coordinate, 144 | * which should be transfered to real angle degrees before using. 145 | */ 146 | void QuadrupedRobot::servo_write_angs(float *angs_ptr, bool is_in_degrees) 147 | { 148 | float real_ang{}; 149 | float pulsewidth{}; 150 | float ang_in_degree{}; 151 | for (size_t i = 0; i < DOF; i++) 152 | { 153 | if (is_in_degrees){ 154 | ang_in_degree = angs_ptr[i]; 155 | } else { 156 | ang_in_degree = angs_ptr[i] * 180 / M_PI; 157 | } 158 | real_ang = init_servo_deg[i] + servo_dir[i] * ang_in_degree; 159 | pulsewidth = 500 + real_ang / 90.0 * 1000; 160 | if (!servos_ptr[i]->attached()) 161 | { 162 | servos_ptr[i]->attach(servo_pins[i], SERVOMIN, SERVOMAX); 163 | } 164 | servos_ptr[i]->writeMicroseconds(pulsewidth); 165 | // update current angles buffer 166 | if (is_in_degrees) { 167 | joint_angs_pre[i] = angs_ptr[i] * M_PI / 180; 168 | } else { 169 | joint_angs_pre[i] = angs_ptr[i]; 170 | } 171 | } 172 | } 173 | 174 | /** 175 | * Moving body by moving the feet in opposite direction 176 | * Body Frame xyz 177 | */ 178 | void QuadrupedRobot::body_xyz(float x, float y, float z) 179 | { 180 | for (size_t i = 0; i < 4; i++) 181 | { 182 | foot_pos[3 * i] = -x; 183 | foot_pos[3 * i + 1] = -y; 184 | foot_pos[3 * i + 2] = -z; 185 | } 186 | inverse_kinematics(); 187 | servo_move(joint_angs_new); 188 | } 189 | 190 | /** 191 | * Moving Body from current position 192 | */ 193 | void QuadrupedRobot::body_move_xyz(float dx, float dy, float dz) 194 | { 195 | for (size_t i = 0; i < 4; i++) 196 | { 197 | foot_pos[3 * i] -= dx; 198 | foot_pos[3 * i + 1] -= dy; 199 | foot_pos[3 * i + 2] -= dz; 200 | } 201 | inverse_kinematics(); 202 | servo_move(joint_angs_new); 203 | } 204 | 205 | /** 206 | * Turn body 207 | */ 208 | void QuadrupedRobot::body_turn_left() 209 | { 210 | // FL pose 211 | foot_pos[0] = - step_turn[0]; 212 | foot_pos[2] = - step_turn[2]; 213 | // RL pose 214 | foot_pos[9] = - step_turn[9]; 215 | foot_pos[11] = - step_turn[11]; 216 | // FR pose 217 | foot_pos[3] = 0; 218 | foot_pos[5] = 0; 219 | // RR pose 220 | foot_pos[6] = 0; 221 | foot_pos[8] = 0; 222 | body_move_xyz(0, 0, 2 * toe_out0); 223 | } 224 | 225 | /** 226 | * Move ith foot with one step, in the same hight work plane 227 | */ 228 | void QuadrupedRobot::foot_step(int i, float x, float z) 229 | { 230 | foot_move_xyz(i, 0, 50, 0); // leg lifting 231 | foot_move_xyz(i, x, 0, z); // motion forward 232 | foot_move_xyz(i, 0, -50, 0); // leg placement 233 | } 234 | 235 | /** 236 | * Move ith foot with a turn angle 237 | */ 238 | void QuadrupedRobot::foot_step_ang(int i) 239 | { 240 | foot_move_xyz(i, 0, 50, 0); // leg lifting 241 | foot_move_xyz(i, step_turn[3 * i], 0, step_turn[3 * i + 2]); 242 | foot_move_xyz(i, 0, -50, 0); // leg placement 243 | } 244 | 245 | /** 246 | * Moving foot from current position 247 | */ 248 | void QuadrupedRobot::foot_move_xyz(int i, float dx, float dy, float dz) 249 | { 250 | foot_pos[3 * i] += dx; 251 | foot_pos[3 * i + 1] += dy; 252 | foot_pos[3 * i + 2] += dz; 253 | inverse_kinematics(); 254 | servo_write_angs(joint_angs_new, false); 255 | delay(200); 256 | } 257 | 258 | /** 259 | * Calculate step_turn 260 | * foot sequence: FL FR RR RL 261 | * index: ith (foot) * <0(x) | 1(y) | 2(z)> 262 | */ 263 | void QuadrupedRobot::turn_pose(float ang) 264 | { 265 | step_turn[0] = body_radius * cos(body_phi0 + ang * M_PI / 180) - feet_dist_x / 2; 266 | step_turn[2] = feet_dist_z / 2 - body_radius * sin(body_phi0 + ang * M_PI / 180); 267 | step_turn[3] = body_radius * cos(body_phi0 - ang * M_PI / 180) - feet_dist_x / 2; 268 | step_turn[5] = body_radius * sin(body_phi0 - ang * M_PI / 180) - feet_dist_z / 2; 269 | step_turn[6] = feet_dist_x / 2 - body_radius * cos(body_phi0 + ang * M_PI / 180); 270 | step_turn[8] = body_radius * sin(body_phi0 + ang * M_PI / 180) - feet_dist_z / 2; 271 | step_turn[9] = feet_dist_x / 2 - body_radius * cos(body_phi0 - ang * M_PI / 180); 272 | step_turn[11] = feet_dist_z / 2 - body_radius * sin(body_phi0 - ang * M_PI / 180); 273 | 274 | // Serial.println(step_turn[0]); 275 | // Serial.println(step_turn[2]); 276 | // Serial.println(step_turn[3]); 277 | // Serial.println(step_turn[5]); 278 | // Serial.println(step_turn[6]); 279 | // Serial.println(step_turn[8]); 280 | // Serial.println(step_turn[9]); 281 | // Serial.println(step_turn[11]); 282 | } 283 | 284 | 285 | /** 286 | * Calculate the joint angles by foot positions 287 | * new angles will be write in joint_angs_new 288 | */ 289 | void QuadrupedRobot::inverse_kinematics() 290 | { 291 | // FL 0, 1, 2 292 | joint_angs_new[0] = gamma_left(foot_pos[1], foot_pos[2]); // HipX 293 | vlegs_len[0] = vleg_left(foot_pos[0], foot_pos[1], joint_angs_new[0]); 294 | joint_angs_new[2] = beta_front(vlegs_len[0]); 295 | joint_angs_new[1] = alfa_front(foot_pos[0], joint_angs_new[2], vlegs_len[0]); 296 | 297 | // FR 3, 4, 5 298 | joint_angs_new[3] = gamma_right(foot_pos[4], foot_pos[5]); 299 | vlegs_len[1] = vleg_right(foot_pos[3], foot_pos[4], joint_angs_new[3]); 300 | joint_angs_new[5] = beta_front(vlegs_len[1]); 301 | joint_angs_new[4] = alfa_front(foot_pos[3], joint_angs_new[5], vlegs_len[1]); 302 | 303 | // RR 6, 7, 8 304 | joint_angs_new[6] = gamma_right(foot_pos[7], foot_pos[8]); 305 | vlegs_len[2] = vleg_right(foot_pos[6], foot_pos[7], joint_angs_new[6]); 306 | joint_angs_new[8] = beta_rear(vlegs_len[2]); 307 | joint_angs_new[7] = alfa_rear(foot_pos[6], joint_angs_new[8], vlegs_len[2]); 308 | 309 | // RL 9, 10, 11 310 | joint_angs_new[9] = gamma_left(foot_pos[10], foot_pos[11]); 311 | vlegs_len[3] = vleg_left(foot_pos[9], foot_pos[10], joint_angs_new[9]); 312 | joint_angs_new[11] = beta_rear(vlegs_len[3]); 313 | joint_angs_new[10] = alfa_rear(foot_pos[9], joint_angs_new[11], vlegs_len[3]); 314 | 315 | // Serial.println("------- Test Angle list: ----------"); 316 | // Serial.println(joint_angs_new[0] * 180 / M_PI); 317 | // Serial.println(joint_angs_new[1] * 180 / M_PI); 318 | // Serial.println(joint_angs_new[2] * 180 / M_PI); 319 | // Serial.println(joint_angs_new[3] * 180 / M_PI); 320 | // Serial.println(joint_angs_new[4] * 180 / M_PI); 321 | // Serial.println(joint_angs_new[5] * 180 / M_PI); 322 | // Serial.println(joint_angs_new[6] * 180 / M_PI); 323 | // Serial.println(joint_angs_new[7] * 180 / M_PI); 324 | // Serial.println(joint_angs_new[8] * 180 / M_PI); 325 | // Serial.println(joint_angs_new[9] * 180 / M_PI); 326 | // Serial.println(joint_angs_new[10] * 180 / M_PI); 327 | // Serial.println(joint_angs_new[11] * 180 / M_PI); 328 | } 329 | 330 | // Gamma: Hip Z angle 331 | float QuadrupedRobot::gamma_left(float dy, float dz) 332 | { 333 | float res = atan((toe_out0 - dz) / (height0 - dy)) - gamma0; 334 | return res; 335 | } 336 | 337 | float QuadrupedRobot::gamma_right(float dy, float dz) 338 | { 339 | float res = gamma0 - atan((toe_out0 + dz) / (height0 - dy)); 340 | return res; 341 | } 342 | // virtual leg length 343 | float QuadrupedRobot::vleg_left(float dx, float dy, float gamma) 344 | { 345 | float res = sqrt(pow(vleg_len0 - (dy / cos(gamma0 + gamma)), 2) + pow(dx, 2)); 346 | if (res > thigh + calf) 347 | res = thigh + calf; 348 | return res; 349 | } 350 | 351 | float QuadrupedRobot::vleg_right(float dx, float dy, float gamma) 352 | { 353 | float res = sqrt(pow(vleg_len0 - (dy / cos(gamma0 - gamma)), 2) + pow(dx, 2)); 354 | if (res > thigh + calf) 355 | res = thigh + calf; 356 | return res; 357 | } 358 | 359 | float QuadrupedRobot::beta_front(float vleg_len) 360 | { 361 | float res = M_PI - acos( 362 | (thigh * thigh + calf * calf - vleg_len * vleg_len) / 363 | (2 * thigh * calf)); 364 | return res; 365 | } 366 | 367 | float QuadrupedRobot::beta_rear(float vleg_len) 368 | { 369 | float res = acos( 370 | (thigh * thigh + calf * calf - vleg_len * vleg_len) / 371 | (2 * thigh * calf)) - M_PI; 372 | return res; 373 | } 374 | 375 | float QuadrupedRobot::alfa_front(float dx, float beta, float vleg_len) 376 | { 377 | float res = asin(dx / vleg_len); 378 | res -= acos( 379 | (thigh * thigh + vleg_len * vleg_len - calf * calf) / 380 | (2 * thigh * vleg_len) 381 | ); 382 | return res; 383 | } 384 | 385 | float QuadrupedRobot::alfa_rear(float dx, float beta, float vleg_len) 386 | { 387 | float res = asin(dx / vleg_len); 388 | res += acos( 389 | (thigh * thigh + vleg_len * vleg_len - calf * calf) / 390 | (2 * thigh * vleg_len) 391 | ); 392 | return res; 393 | } 394 | 395 | /** 396 | * Only used for adjusting legs 397 | */ 398 | void QuadrupedRobot::adjust() 399 | { 400 | servo_write_angs(adjust_angs, true); 401 | clear_cmd(); 402 | } 403 | 404 | /** 405 | * Stand up 406 | * Non-periodic 407 | */ 408 | void QuadrupedRobot::bot_stand() 409 | { 410 | // servo_write_angs(stand_angs, true); 411 | body_xyz(0, 0, 0); 412 | clear_cmd(); 413 | } 414 | 415 | /** 416 | * Sleep mode 417 | * Non-periodic 418 | */ 419 | void QuadrupedRobot::bot_rest() 420 | { 421 | servo_write_angs(rest_angs, true); 422 | clear_cmd(); 423 | } 424 | 425 | /** 426 | * walk 427 | * Periodic, running til new interrupt 428 | */ 429 | void QuadrupedRobot::bot_walk() 430 | { 431 | Serial.println("walk"); 432 | body_move_xyz(steplen / 4, 0, toe_out0); 433 | foot_step(3, steplen / 2, 0); // move FL 434 | foot_step(0, steplen / 2, 0); // move FL 435 | body_move_xyz(steplen / 4, 0, -2 * toe_out0); 436 | while (command_code == 3) 437 | { 438 | foot_step(2, steplen, 0); // move RR 439 | foot_step(1, steplen, 0); // move FR 440 | body_move_xyz(steplen / 2, 0, 2 * toe_out0 ); 441 | foot_step(3, steplen, 0); // move RL 442 | foot_step(0, steplen, 0); // move FL 443 | body_move_xyz(steplen / 2, 0, -2 * toe_out0); 444 | interrupt(); 445 | } 446 | foot_step(2, steplen / 2, 0); 447 | foot_step(1, steplen / 2, 0); 448 | body_xyz(0, 0, 0); 449 | delay(500); 450 | } 451 | 452 | void QuadrupedRobot::bot_turn_left() 453 | { 454 | Serial.println("turn right"); 455 | turn_pose(turn_phi); 456 | body_xyz(toe_out0 / 2, 0, -toe_out0); // Lean left 457 | foot_step_ang(2); // move RR 458 | foot_step_ang(1); // move FR 459 | body_turn_left(); // move body 460 | foot_step_ang(0); // move FL 461 | // foot_step_ang(3); // move RL 462 | 463 | pause(); 464 | } 465 | 466 | void QuadrupedRobot::bot_turn_right() 467 | { 468 | Serial.println("turn left"); 469 | while (command_code == 5) 470 | { 471 | ; 472 | } 473 | 474 | } 475 | 476 | void QuadrupedRobot::pause() 477 | { 478 | while(!interrupt()) 479 | { 480 | delay(1000); 481 | } 482 | } 483 | 484 | QuadrupedRobot::~QuadrupedRobot() 485 | { 486 | for (size_t i = 0; i < DOF; i++) 487 | { 488 | delete servos_ptr[i]; 489 | } 490 | } 491 | -------------------------------------------------------------------------------- /webots_sim/worlds/mini_dog.wbt: -------------------------------------------------------------------------------- 1 | #VRML_SIM R2019b utf8 2 | WorldInfo { 3 | basicTimeStep 16 4 | contactProperties [ 5 | ContactProperties { 6 | material2 "dog" 7 | } 8 | ] 9 | } 10 | Viewpoint { 11 | orientation 0.018359396027182024 -0.9887732863130595 -0.1482913377483308 3.4263965992275334 12 | position 0.4271495640178565 0.23288160385467133 -1.2576271803901207 13 | } 14 | TexturedBackground { 15 | } 16 | TexturedBackgroundLight { 17 | } 18 | RectangleArena { 19 | translation 0 -0.3 0.19 20 | rotation 1 0 0 4.692820414042842e-06 21 | floorSize 10 10 22 | } 23 | DEF Mini_Dog Robot { 24 | translation 0 -0.06 0 25 | scale 1.00156 1.00156 1.00156 26 | children [ 27 | DEF HIP4__JOINT Hinge2Joint { 28 | jointParameters HingeJointParameters { 29 | anchor -0.1 0 -0.05 30 | } 31 | jointParameters2 JointParameters { 32 | } 33 | device [ 34 | RotationalMotor { 35 | name "servo_hip4x" 36 | } 37 | ] 38 | device2 [ 39 | RotationalMotor { 40 | name "servo_hip4z" 41 | } 42 | ] 43 | endPoint DEF LEG4 Solid { 44 | translation -0.1000217964229049 2.8992818645565064e-06 -0.050068472792199295 45 | rotation 0.1329708628000711 0.9911199471530925 1.2065134859256913e-06 0.0005692825420769882 46 | children [ 47 | DEF KNEE4_JOINT HingeJoint { 48 | jointParameters HingeJointParameters { 49 | axis 0 0 1 50 | anchor 0 -0.1 0 51 | } 52 | device [ 53 | RotationalMotor { 54 | name "servo_knee4" 55 | } 56 | ] 57 | endPoint DEF LEG3_SPRING Solid { 58 | translation 0 -0.1 0 59 | rotation 0 0 1 0 60 | children [ 61 | DEF FOOT Solid { 62 | translation 0 -0.1 0 63 | rotation 1 0 0 1.5708 64 | children [ 65 | DEF FOOT_SHAPE Shape { 66 | appearance PBRAppearance { 67 | baseColor 0.768627 0.627451 0 68 | roughness 1 69 | metalness 0 70 | } 71 | geometry Cylinder { 72 | height 0.006 73 | radius 0.01 74 | } 75 | } 76 | ] 77 | boundingObject USE FOOT_SHAPE 78 | } 79 | DEF LEG1_SPRING_TRANS Transform { 80 | translation 0 -0.05 0 81 | children [ 82 | DEF LEG_SHAPE Shape { 83 | appearance PBRAppearance { 84 | baseColor 0.768627 0.627451 0 85 | } 86 | geometry Box { 87 | size 0.01 0.1 0.005 88 | } 89 | } 90 | ] 91 | } 92 | DEF KNEE_JOINT_TRANS Transform { 93 | rotation 1 0 0 1.5708 94 | children [ 95 | Shape { 96 | appearance PBRAppearance { 97 | baseColor 0.768627 0.627451 0 98 | } 99 | geometry Cylinder { 100 | height 0.006 101 | radius 0.01 102 | } 103 | } 104 | ] 105 | } 106 | ] 107 | boundingObject USE KNEE_JOINT_TRANS 108 | physics Physics { 109 | } 110 | } 111 | } 112 | DEF LEG41_TRANS Transform { 113 | translation 0 -0.05 0 114 | children [ 115 | DEF LEG_SHAPE Shape { 116 | appearance PBRAppearance { 117 | baseColor 0.768627 0.627451 0 118 | } 119 | geometry Box { 120 | size 0.01 0.1 0.005 121 | } 122 | } 123 | ] 124 | } 125 | DEF HIP_JOINT_TRANS Transform { 126 | rotation 1 0 0 1.5708 127 | children [ 128 | DEF JOINT_SHAPE Shape { 129 | appearance PBRAppearance { 130 | baseColor 0.768627 0.627451 0 131 | } 132 | geometry Cylinder { 133 | height 0.006 134 | radius 0.008 135 | } 136 | } 137 | ] 138 | } 139 | ] 140 | name "leg4" 141 | boundingObject USE HIP_JOINT_TRANS 142 | physics Physics { 143 | } 144 | } 145 | } 146 | DEF HIP3_JOINT Hinge2Joint { 147 | jointParameters HingeJointParameters { 148 | anchor -0.1 0 0.05 149 | } 150 | jointParameters2 JointParameters { 151 | } 152 | device [ 153 | RotationalMotor { 154 | name "servo_hip3x" 155 | } 156 | ] 157 | device2 [ 158 | RotationalMotor { 159 | name "servo_hip3z" 160 | } 161 | ] 162 | endPoint DEF LEG3 Solid { 163 | translation -0.09999810900870515 -2.9604134401916184e-06 0.050015324742532695 164 | rotation -0.4790615631717819 -0.8777813045911933 1.2615298936687952e-06 0.00046450745023388155 165 | children [ 166 | DEF KNEE3_JOINT HingeJoint { 167 | jointParameters HingeJointParameters { 168 | axis 0 0 1 169 | anchor 0 -0.1 0 170 | } 171 | device [ 172 | RotationalMotor { 173 | name "servo_knee3" 174 | } 175 | ] 176 | endPoint DEF LEG3_SPRING Solid { 177 | translation 0 -0.1 0 178 | rotation 0 0 1 0 179 | children [ 180 | DEF FOOT Solid { 181 | translation 0 -0.1 0 182 | rotation 1 0 0 1.5708 183 | children [ 184 | DEF FOOT_SHAPE Shape { 185 | appearance PBRAppearance { 186 | baseColor 0.768627 0.627451 0 187 | roughness 1 188 | metalness 0 189 | } 190 | geometry Cylinder { 191 | height 0.006 192 | radius 0.01 193 | } 194 | } 195 | ] 196 | boundingObject USE FOOT_SHAPE 197 | } 198 | DEF LEG1_SPRING_TRANS Transform { 199 | translation 0 -0.05 0 200 | children [ 201 | DEF LEG_SHAPE Shape { 202 | appearance PBRAppearance { 203 | baseColor 0.768627 0.627451 0 204 | } 205 | geometry Box { 206 | size 0.01 0.1 0.005 207 | } 208 | } 209 | ] 210 | } 211 | DEF KNEE_JOINT_TRANS Transform { 212 | rotation 1 0 0 1.5708 213 | children [ 214 | Shape { 215 | appearance PBRAppearance { 216 | baseColor 0.768627 0.627451 0 217 | } 218 | geometry Cylinder { 219 | height 0.006 220 | radius 0.01 221 | } 222 | } 223 | ] 224 | } 225 | ] 226 | boundingObject USE KNEE_JOINT_TRANS 227 | physics Physics { 228 | } 229 | } 230 | } 231 | DEF LEG31_TRANS Transform { 232 | translation 0 -0.05 0 233 | children [ 234 | DEF LEG_SHAPE Shape { 235 | appearance PBRAppearance { 236 | baseColor 0.768627 0.627451 0 237 | } 238 | geometry Box { 239 | size 0.01 0.1 0.005 240 | } 241 | } 242 | ] 243 | } 244 | DEF HIP_JOINT_TRANS Transform { 245 | rotation 1 0 0 1.5708 246 | children [ 247 | DEF JOINT_SHAPE Shape { 248 | appearance PBRAppearance { 249 | baseColor 0.768627 0.627451 0 250 | } 251 | geometry Cylinder { 252 | height 0.006 253 | radius 0.008 254 | } 255 | } 256 | ] 257 | } 258 | ] 259 | name "leg3" 260 | boundingObject USE HIP_JOINT_TRANS 261 | physics Physics { 262 | } 263 | } 264 | } 265 | DEF HIP2_JOINT Hinge2Joint { 266 | jointParameters HingeJointParameters { 267 | anchor 0.1 0 0.05 268 | } 269 | jointParameters2 JointParameters { 270 | } 271 | device [ 272 | RotationalMotor { 273 | name "servo_hip2x" 274 | } 275 | ] 276 | device2 [ 277 | RotationalMotor { 278 | name "servo_hip2z" 279 | } 280 | ] 281 | endPoint DEF LEG2 Solid { 282 | translation 0.10002134609802116 -3.484747990434354e-06 0.05006762933791575 283 | rotation -0.15247478463497133 0.988307361112047 -4.718177095537075e-06 0.00041318025708980086 284 | children [ 285 | DEF KNEE2_JOINT HingeJoint { 286 | jointParameters HingeJointParameters { 287 | axis 0 0 1 288 | anchor 0 -0.1 0 289 | } 290 | device [ 291 | RotationalMotor { 292 | name "servo_knee2" 293 | } 294 | ] 295 | endPoint DEF LEG2_SPRING Solid { 296 | translation 0 -0.1 0 297 | rotation 0 0 1 0 298 | children [ 299 | DEF FOOT Solid { 300 | translation 0 -0.1 0 301 | rotation 1 0 0 1.5708 302 | children [ 303 | DEF FOOT_SHAPE Shape { 304 | appearance PBRAppearance { 305 | baseColor 0.768627 0.627451 0 306 | roughness 1 307 | metalness 0 308 | } 309 | geometry Cylinder { 310 | height 0.006 311 | radius 0.01 312 | } 313 | } 314 | ] 315 | boundingObject USE FOOT_SHAPE 316 | } 317 | DEF LEG1_SPRING_TRANS Transform { 318 | translation 0 -0.05 0 319 | children [ 320 | DEF LEG_SHAPE Shape { 321 | appearance PBRAppearance { 322 | baseColor 0.768627 0.627451 0 323 | } 324 | geometry Box { 325 | size 0.01 0.1 0.005 326 | } 327 | } 328 | ] 329 | } 330 | DEF KNEE_JOINT_TRANS Transform { 331 | rotation 1 0 0 1.5708 332 | children [ 333 | Shape { 334 | appearance PBRAppearance { 335 | baseColor 0.768627 0.627451 0 336 | } 337 | geometry Cylinder { 338 | height 0.006 339 | radius 0.01 340 | } 341 | } 342 | ] 343 | } 344 | ] 345 | boundingObject USE KNEE_JOINT_TRANS 346 | physics Physics { 347 | } 348 | } 349 | } 350 | DEF LEG21_TRANS Transform { 351 | translation 0 -0.05 0 352 | children [ 353 | DEF LEG_SHAPE Shape { 354 | appearance PBRAppearance { 355 | baseColor 0.768627 0.627451 0 356 | } 357 | geometry Box { 358 | size 0.01 0.1 0.005 359 | } 360 | } 361 | ] 362 | } 363 | DEF HIP_JOINT_TRANS Transform { 364 | rotation 1 0 0 1.5708 365 | children [ 366 | DEF JOINT_SHAPE Shape { 367 | appearance PBRAppearance { 368 | baseColor 0.768627 0.627451 0 369 | } 370 | geometry Cylinder { 371 | height 0.006 372 | radius 0.008 373 | } 374 | } 375 | ] 376 | } 377 | ] 378 | name "leg2" 379 | boundingObject USE HIP_JOINT_TRANS 380 | physics Physics { 381 | } 382 | } 383 | } 384 | DEF HIP1_JOINT Hinge2Joint { 385 | jointParameters HingeJointParameters { 386 | anchor 0.1 0 -0.05 387 | } 388 | jointParameters2 JointParameters { 389 | } 390 | device [ 391 | RotationalMotor { 392 | name "servo_hip1x" 393 | } 394 | ] 395 | device2 [ 396 | RotationalMotor { 397 | name "servo_hip1z" 398 | } 399 | ] 400 | endPoint DEF LEG1 Solid { 401 | translation 0.09999358285568746 6.3633680852749e-06 -0.0500161543818609 402 | rotation 0.382140124004998 -0.9241043910675198 5.930683902590514e-06 0.00047789221437204897 403 | children [ 404 | DEF KNEE1_JOINT HingeJoint { 405 | jointParameters HingeJointParameters { 406 | axis 0 0 1 407 | anchor 0 -0.1 0 408 | } 409 | device [ 410 | RotationalMotor { 411 | name "servo_knee1" 412 | } 413 | ] 414 | endPoint DEF LEG1_SPRING Solid { 415 | translation 0 -0.1 0 416 | rotation 0 0 1 0 417 | children [ 418 | DEF FOOT Solid { 419 | translation 0 -0.1 0 420 | rotation 1 0 0 1.5708 421 | children [ 422 | DEF FOOT_SHAPE Shape { 423 | appearance PBRAppearance { 424 | baseColor 0.768627 0.627451 0 425 | roughness 1 426 | metalness 0 427 | } 428 | geometry Cylinder { 429 | height 0.006 430 | radius 0.01 431 | } 432 | } 433 | ] 434 | boundingObject USE FOOT_SHAPE 435 | } 436 | DEF LEG1_SPRING_TRANS Transform { 437 | translation 0 -0.05 0 438 | children [ 439 | DEF LEG_SHAPE Shape { 440 | appearance PBRAppearance { 441 | baseColor 0.768627 0.627451 0 442 | } 443 | geometry Box { 444 | size 0.01 0.1 0.005 445 | } 446 | } 447 | ] 448 | } 449 | DEF KNEE_JOINT_TRANS Transform { 450 | rotation 1 0 0 1.5708 451 | children [ 452 | Shape { 453 | appearance PBRAppearance { 454 | baseColor 0.768627 0.627451 0 455 | } 456 | geometry Cylinder { 457 | height 0.006 458 | radius 0.01 459 | } 460 | } 461 | ] 462 | } 463 | ] 464 | boundingObject USE KNEE_JOINT_TRANS 465 | physics Physics { 466 | } 467 | } 468 | } 469 | DEF LEG11_TRANS Transform { 470 | translation 0 -0.05 0 471 | children [ 472 | DEF LEG_SHAPE Shape { 473 | appearance PBRAppearance { 474 | baseColor 0.768627 0.627451 0 475 | } 476 | geometry Box { 477 | size 0.01 0.1 0.005 478 | } 479 | } 480 | ] 481 | } 482 | DEF HIP_JOINT_TRANS Transform { 483 | rotation 1 0 0 1.5708 484 | children [ 485 | DEF JOINT_SHAPE Shape { 486 | appearance PBRAppearance { 487 | baseColor 0.768627 0.627451 0 488 | } 489 | geometry Cylinder { 490 | height 0.006 491 | radius 0.008 492 | } 493 | } 494 | ] 495 | } 496 | ] 497 | boundingObject USE HIP_JOINT_TRANS 498 | physics Physics { 499 | } 500 | } 501 | } 502 | DEF BODY_SHAPE Shape { 503 | appearance PBRAppearance { 504 | baseColor 0.305882 0.603922 0.0235294 505 | roughness 0.8 506 | metalness 0.1 507 | } 508 | geometry Box { 509 | size 0.26 0.02 0.093 510 | } 511 | } 512 | ] 513 | name "mini dog" 514 | boundingObject USE BODY_SHAPE 515 | physics Physics { 516 | } 517 | controller "cpg_hopf_controller" 518 | } 519 | MyWorkbanch { 520 | translation 0 -0.171728 0 521 | scale 0.6 0.6 0.6 522 | } 523 | -------------------------------------------------------------------------------- /webots_sim/protos/DotGo.proto: -------------------------------------------------------------------------------- 1 | PROTO DotGo [ 2 | field SFVec3f translation 0 0.33 0 3 | field SFRotation rotation 0 1 0 0 4 | field SFString controller "init_testing" 5 | ] 6 | { 7 | Robot { 8 | translation IS translation 9 | rotation IS rotation 10 | children [ 11 | DEF HIP3 Hinge2Joint { 12 | jointParameters HingeJointParameters { 13 | anchor -0.2 0 -0.1 14 | } 15 | jointParameters2 JointParameters { 16 | } 17 | device [ 18 | PositionSensor { 19 | name "hip3x_pos" 20 | } 21 | RotationalMotor { 22 | name "hip3x" 23 | } 24 | ] 25 | device2 [ 26 | PositionSensor { 27 | name "hip3z_pos" 28 | } 29 | RotationalMotor { 30 | name "hip3z" 31 | } 32 | ] 33 | endPoint Solid { 34 | translation -0.2 0 -0.1 35 | children [ 36 | DEF KNEE3 HingeJoint { 37 | jointParameters HingeJointParameters { 38 | axis 0 0 1 39 | anchor 0 -0.15 0 40 | } 41 | device [ 42 | PositionSensor { 43 | name "knee3_pos" 44 | } 45 | RotationalMotor { 46 | name "knee3" 47 | } 48 | ] 49 | endPoint Solid { 50 | translation 0 -0.15 0 51 | rotation 0 0 1 0 52 | children [ 53 | DEF KNEE1_GROUP Group { 54 | children [ 55 | DEF FOOT Transform { 56 | translation 0 -0.15 0 57 | rotation 1 0 0 1.5707 58 | children [ 59 | Shape { 60 | appearance PBRAppearance { 61 | baseColor 0.180392 0.203922 0.211765 62 | roughness 0.5 63 | metalness 0 64 | } 65 | geometry Cylinder { 66 | height 0.05 67 | radius 0.03 68 | } 69 | } 70 | ] 71 | } 72 | DEF SPRING Transform { 73 | translation 0 -0.075 0 74 | children [ 75 | Shape { 76 | appearance PBRAppearance { 77 | baseColor 0 0.8 0 78 | roughness 0.5 79 | } 80 | geometry Cylinder { 81 | height 0.15 82 | radius 0.015 83 | } 84 | } 85 | ] 86 | } 87 | DEF KNEE Transform { 88 | rotation 1 0 0 1.5707 89 | children [ 90 | Shape { 91 | appearance PBRAppearance { 92 | baseColor 0.8 0 0 93 | roughness 0.5 94 | } 95 | geometry Cylinder { 96 | height 0.03 97 | radius 0.03 98 | } 99 | } 100 | ] 101 | } 102 | ] 103 | } 104 | ] 105 | boundingObject USE KNEE1_GROUP 106 | physics Physics { 107 | } 108 | } 109 | } 110 | DEF HIP3_GROUP Group { 111 | children [ 112 | DEF FEMUR_TRANS Transform { 113 | translation 0 -0.075 0 114 | children [ 115 | Shape { 116 | appearance PBRAppearance { 117 | baseColor 0 0.8 0 118 | roughness 0.5 119 | } 120 | geometry Cylinder { 121 | height 0.15 122 | radius 0.015 123 | } 124 | } 125 | ] 126 | } 127 | DEF HIP1_TRANS Transform { 128 | children [ 129 | Shape { 130 | appearance PBRAppearance { 131 | baseColor 0.8 0 0 132 | roughness 0.5 133 | } 134 | geometry Sphere { 135 | radius 0.03 136 | } 137 | } 138 | ] 139 | } 140 | ] 141 | } 142 | ] 143 | name "hip3" 144 | boundingObject USE HIP3_GROUP 145 | physics Physics { 146 | } 147 | } 148 | } 149 | DEF HIP2 Hinge2Joint { 150 | jointParameters HingeJointParameters { 151 | anchor -0.2 0 0.1 152 | } 153 | jointParameters2 JointParameters { 154 | } 155 | device [ 156 | PositionSensor { 157 | name "hip2x_pos" 158 | } 159 | RotationalMotor { 160 | name "hip2x" 161 | } 162 | ] 163 | device2 [ 164 | PositionSensor { 165 | name "hip2z_pos" 166 | } 167 | RotationalMotor { 168 | name "hip2z" 169 | } 170 | ] 171 | endPoint Solid { 172 | translation -0.2 0 0.1 173 | children [ 174 | DEF KNEE2 HingeJoint { 175 | jointParameters HingeJointParameters { 176 | axis 0 0 1 177 | anchor 0 -0.15 0 178 | } 179 | device [ 180 | PositionSensor { 181 | name "knee2_pos" 182 | } 183 | RotationalMotor { 184 | name "knee2" 185 | } 186 | ] 187 | endPoint Solid { 188 | translation 0 -0.15 0 189 | rotation 0 0 1 0 190 | children [ 191 | DEF KNEE2_GROUP Group { 192 | children [ 193 | DEF FOOT Transform { 194 | translation 0 -0.15 0 195 | rotation 1 0 0 1.5707 196 | children [ 197 | Shape { 198 | appearance PBRAppearance { 199 | baseColor 0.180392 0.203922 0.211765 200 | roughness 0.5 201 | metalness 0 202 | } 203 | geometry Cylinder { 204 | height 0.05 205 | radius 0.03 206 | } 207 | } 208 | ] 209 | } 210 | DEF SPRING Transform { 211 | translation 0 -0.075 0 212 | children [ 213 | Shape { 214 | appearance PBRAppearance { 215 | baseColor 0 0.8 0 216 | roughness 0.5 217 | } 218 | geometry Cylinder { 219 | height 0.15 220 | radius 0.015 221 | } 222 | } 223 | ] 224 | } 225 | DEF KNEE Transform { 226 | rotation 1 0 0 1.5707 227 | children [ 228 | Shape { 229 | appearance PBRAppearance { 230 | baseColor 0.8 0 0 231 | roughness 1 232 | } 233 | geometry Cylinder { 234 | height 0.03 235 | radius 0.03 236 | } 237 | } 238 | ] 239 | } 240 | ] 241 | } 242 | ] 243 | boundingObject USE KNEE2_GROUP 244 | physics Physics { 245 | } 246 | } 247 | } 248 | DEF HIP2_GROUP Group { 249 | children [ 250 | DEF FEMUR_TRANS Transform { 251 | translation 0 -0.075 0 252 | children [ 253 | Shape { 254 | appearance PBRAppearance { 255 | baseColor 0 0.8 0 256 | roughness 0.5 257 | } 258 | geometry Cylinder { 259 | height 0.15 260 | radius 0.015 261 | } 262 | } 263 | ] 264 | } 265 | DEF HIP2_TRANS Transform { 266 | children [ 267 | Shape { 268 | appearance PBRAppearance { 269 | baseColor 0.8 0 0 270 | roughness 0.5 271 | } 272 | geometry Sphere { 273 | radius 0.03 274 | } 275 | } 276 | ] 277 | } 278 | ] 279 | } 280 | ] 281 | name "hip2" 282 | boundingObject USE HIP2_GROUP 283 | physics Physics { 284 | } 285 | } 286 | } 287 | DEF HIP1 Hinge2Joint { 288 | jointParameters HingeJointParameters { 289 | anchor 0.2 0 -0.1 290 | } 291 | jointParameters2 JointParameters { 292 | } 293 | device [ 294 | PositionSensor { 295 | name "hip1x_pos" 296 | } 297 | RotationalMotor { 298 | name "hip1x" 299 | } 300 | ] 301 | device2 [ 302 | PositionSensor { 303 | name "hip1z_pos" 304 | } 305 | RotationalMotor { 306 | name "hip1z" 307 | maxTorque 100 308 | } 309 | ] 310 | endPoint Solid { 311 | translation 0.2 0 -0.1 312 | children [ 313 | DEF KNEE1 HingeJoint { 314 | jointParameters HingeJointParameters { 315 | axis 0 0 1 316 | anchor 0 -0.15 0 317 | } 318 | device [ 319 | PositionSensor { 320 | name "knee1_pos" 321 | } 322 | RotationalMotor { 323 | name "knee1" 324 | } 325 | ] 326 | endPoint Solid { 327 | translation 0 -0.15 0 328 | rotation 0 0 1 0 329 | children [ 330 | DEF KNEE1_GROUP Group { 331 | children [ 332 | DEF FOOT Transform { 333 | translation 0 -0.15 0 334 | rotation 1 0 0 1.5707 335 | children [ 336 | Shape { 337 | appearance PBRAppearance { 338 | baseColor 0.180392 0.203922 0.211765 339 | roughness 0.5 340 | metalness 0 341 | } 342 | geometry Cylinder { 343 | height 0.05 344 | radius 0.03 345 | } 346 | } 347 | ] 348 | } 349 | DEF SPRING Transform { 350 | translation 0 -0.075 0 351 | children [ 352 | Shape { 353 | appearance PBRAppearance { 354 | baseColor 0 0.8 0 355 | roughness 0.5 356 | } 357 | geometry Cylinder { 358 | height 0.15 359 | radius 0.015 360 | } 361 | } 362 | ] 363 | } 364 | DEF KNEE Transform { 365 | rotation 1 0 0 1.5707 366 | children [ 367 | Shape { 368 | appearance PBRAppearance { 369 | baseColor 0.8 0 0 370 | roughness 1 371 | } 372 | geometry Cylinder { 373 | height 0.03 374 | radius 0.03 375 | } 376 | } 377 | ] 378 | } 379 | ] 380 | } 381 | ] 382 | boundingObject USE KNEE1_GROUP 383 | physics Physics { 384 | } 385 | } 386 | } 387 | DEF HIP1_GROUP Group { 388 | children [ 389 | DEF FEMUR_TRANS Transform { 390 | translation 0 -0.075 0 391 | children [ 392 | Shape { 393 | appearance PBRAppearance { 394 | baseColor 0 0.8 0 395 | roughness 0.5 396 | } 397 | geometry Cylinder { 398 | height 0.15 399 | radius 0.015 400 | } 401 | } 402 | ] 403 | } 404 | DEF HIP1_TRANS Transform { 405 | children [ 406 | Shape { 407 | appearance PBRAppearance { 408 | baseColor 0.8 0 0 409 | roughness 0.5 410 | } 411 | geometry Sphere { 412 | radius 0.03 413 | } 414 | } 415 | ] 416 | } 417 | ] 418 | } 419 | ] 420 | name "hip1" 421 | boundingObject USE HIP1_GROUP 422 | physics Physics { 423 | } 424 | } 425 | } 426 | DEF HIP0 Hinge2Joint { 427 | jointParameters HingeJointParameters { 428 | anchor 0.2 0 0.1 429 | } 430 | jointParameters2 JointParameters { 431 | } 432 | device [ 433 | PositionSensor { 434 | name "hip0x_pos" 435 | } 436 | RotationalMotor { 437 | name "hip0x" 438 | } 439 | ] 440 | device2 [ 441 | PositionSensor { 442 | name "hip0z_pos" 443 | } 444 | RotationalMotor { 445 | name "hip0z" 446 | } 447 | ] 448 | endPoint Solid { 449 | translation 0.2 0 0.1 450 | children [ 451 | DEF KNEE0 HingeJoint { 452 | jointParameters HingeJointParameters { 453 | axis 0 0 1 454 | anchor 0 -0.15 0 455 | } 456 | device [ 457 | PositionSensor { 458 | name "knee0_pos" 459 | } 460 | RotationalMotor { 461 | name "knee0" 462 | } 463 | ] 464 | endPoint Solid { 465 | translation 0 -0.15 0 466 | rotation 0 0 1 0 467 | children [ 468 | DEF KNEE0_GROUP Group { 469 | children [ 470 | DEF KNEE Transform { 471 | rotation 1 0 0 1.5707 472 | children [ 473 | DEF KNEE_SHAPE Shape { 474 | appearance PBRAppearance { 475 | baseColor 0.8 0 0 476 | roughness 0.5 477 | } 478 | geometry Cylinder { 479 | height 0.03 480 | radius 0.03 481 | } 482 | } 483 | ] 484 | } 485 | DEF FOOT0 Transform { 486 | translation 0 -0.15 0 487 | rotation 1 0 0 1.5707 488 | children [ 489 | Shape { 490 | appearance PBRAppearance { 491 | baseColor 0.180392 0.203922 0.211765 492 | roughness 0.5 493 | metalness 0.5 494 | } 495 | geometry Cylinder { 496 | height 0.05 497 | radius 0.03 498 | } 499 | } 500 | ] 501 | } 502 | DEF SPRING Transform { 503 | translation 0 -0.075 0 504 | rotation 0 0 1 0 505 | children [ 506 | Shape { 507 | appearance PBRAppearance { 508 | baseColor 0 0.8 0 509 | roughness 0.5 510 | } 511 | geometry Cylinder { 512 | height 0.15 513 | radius 0.015 514 | } 515 | } 516 | ] 517 | } 518 | ] 519 | } 520 | ] 521 | boundingObject USE KNEE0_GROUP 522 | physics Physics { 523 | } 524 | } 525 | } 526 | DEF HIP0_GROUP Group { 527 | children [ 528 | DEF FEMUR_TRANS Transform { 529 | translation 0 -0.075 0 530 | children [ 531 | Shape { 532 | appearance DEF LEG_APPEARANCE PBRAppearance { 533 | baseColor 0 0.8 0 534 | roughness 0.5 535 | } 536 | geometry Cylinder { 537 | height 0.15 538 | radius 0.015 539 | } 540 | } 541 | ] 542 | } 543 | DEF HIP_TRANS Transform { 544 | children [ 545 | DEF HIP_SHAPE Shape { 546 | appearance DEF JOINT_APPEARANCE PBRAppearance { 547 | baseColor 0.8 0 0 548 | roughness 0.5 549 | } 550 | geometry Sphere { 551 | radius 0.03 552 | } 553 | } 554 | ] 555 | } 556 | ] 557 | } 558 | ] 559 | name "hip0" 560 | boundingObject USE HIP0_GROUP 561 | physics Physics { 562 | centerOfMass [ 563 | 0 0 0 564 | ] 565 | } 566 | } 567 | } 568 | DEF BODY_SHAPE Shape { 569 | appearance DEF BODY_APPEARANCE PBRAppearance { 570 | baseColor 0.756863 0.490196 0.0666667 571 | roughness 0.5 572 | metalness 0.1 573 | } 574 | geometry Box { 575 | size 0.4 0.05 0.2 576 | } 577 | } 578 | ] 579 | name "dot_go" 580 | contactMaterial "dog" 581 | boundingObject USE BODY_SHAPE 582 | physics Physics { 583 | } 584 | controller IS controller 585 | } 586 | } 587 | -------------------------------------------------------------------------------- /webots_sim/worlds/quadruped_robot.wbt: -------------------------------------------------------------------------------- 1 | #VRML_SIM R2019b utf8 2 | WorldInfo { 3 | basicTimeStep 16 4 | contactProperties [ 5 | ContactProperties { 6 | material2 "dog" 7 | } 8 | ] 9 | } 10 | Viewpoint { 11 | orientation -0.0063886957607947855 -0.9888096300971353 -0.149045966042837 3.0977108911638376 12 | position -0.2197228050910729 1.0664674947282815 -3.580247565412533 13 | } 14 | TexturedBackground { 15 | } 16 | TexturedBackgroundLight { 17 | } 18 | RectangleArena { 19 | translation 0 -0.3 0.19 20 | rotation 1 0 0 4.692820414042842e-06 21 | floorSize 4 4 22 | } 23 | DEF DOT_GO Robot { 24 | translation 0 0.12 0.01 25 | children [ 26 | DEF HIP3 Hinge2Joint { 27 | jointParameters HingeJointParameters { 28 | anchor -0.2 0 -0.1 29 | } 30 | jointParameters2 JointParameters { 31 | } 32 | device [ 33 | PositionSensor { 34 | name "hip3x_pos" 35 | } 36 | RotationalMotor { 37 | name "hip3x" 38 | maxTorque 100 39 | } 40 | ] 41 | device2 [ 42 | PositionSensor { 43 | name "hip3z_pos" 44 | } 45 | RotationalMotor { 46 | name "hip3z" 47 | maxTorque 100 48 | } 49 | ] 50 | endPoint Solid { 51 | translation -0.20000608686423835 -4.766681333544348e-05 -0.09999999754399529 52 | rotation -0.05596291416413469 -0.9984328481366475 2.449044735558207e-09 1.1151007970493858e-07 53 | children [ 54 | DEF KNEE3 HingeJoint { 55 | jointParameters HingeJointParameters { 56 | axis 0 0 1 57 | anchor 0 -0.15 0 58 | } 59 | device [ 60 | PositionSensor { 61 | name "knee3_pos" 62 | } 63 | RotationalMotor { 64 | name "knee3" 65 | maxTorque 100 66 | } 67 | ] 68 | endPoint Solid { 69 | translation 0 -0.15 0 70 | rotation 0 0 1 0 71 | children [ 72 | DEF KNEE1_GROUP Group { 73 | children [ 74 | DEF FOOT Transform { 75 | translation 0 -0.15 0 76 | rotation 1 0 0 1.5707 77 | children [ 78 | Shape { 79 | appearance PBRAppearance { 80 | baseColor 0.180392 0.203922 0.211765 81 | roughness 0.5 82 | metalness 0 83 | } 84 | geometry Cylinder { 85 | height 0.05 86 | radius 0.03 87 | } 88 | } 89 | ] 90 | } 91 | DEF SPRING Transform { 92 | translation 0 -0.075 0 93 | children [ 94 | Shape { 95 | appearance PBRAppearance { 96 | baseColor 0 0.8 0 97 | roughness 0.5 98 | } 99 | geometry Cylinder { 100 | height 0.15 101 | radius 0.015 102 | } 103 | } 104 | ] 105 | } 106 | DEF KNEE Transform { 107 | rotation 1 0 0 1.5707 108 | children [ 109 | Shape { 110 | appearance PBRAppearance { 111 | baseColor 0.8 0 0 112 | roughness 0.5 113 | } 114 | geometry Cylinder { 115 | height 0.03 116 | radius 0.03 117 | } 118 | } 119 | ] 120 | } 121 | ] 122 | } 123 | ] 124 | boundingObject USE KNEE1_GROUP 125 | physics Physics { 126 | } 127 | } 128 | } 129 | DEF HIP3_GROUP Group { 130 | children [ 131 | DEF FEMUR_TRANS Transform { 132 | translation 0 -0.075 0 133 | children [ 134 | Shape { 135 | appearance PBRAppearance { 136 | baseColor 0 0.8 0 137 | roughness 0.5 138 | } 139 | geometry Cylinder { 140 | height 0.15 141 | radius 0.015 142 | } 143 | } 144 | ] 145 | } 146 | DEF HIP1_TRANS Transform { 147 | children [ 148 | Shape { 149 | appearance PBRAppearance { 150 | baseColor 0.8 0 0 151 | roughness 0.5 152 | } 153 | geometry Sphere { 154 | radius 0.03 155 | } 156 | } 157 | ] 158 | } 159 | ] 160 | } 161 | ] 162 | name "hip3" 163 | boundingObject USE HIP3_GROUP 164 | physics Physics { 165 | } 166 | } 167 | } 168 | DEF HIP2 Hinge2Joint { 169 | jointParameters HingeJointParameters { 170 | anchor -0.2 0 0.1 171 | } 172 | jointParameters2 JointParameters { 173 | } 174 | device [ 175 | PositionSensor { 176 | name "hip2x_pos" 177 | } 178 | RotationalMotor { 179 | name "hip2x" 180 | maxTorque 100 181 | } 182 | ] 183 | device2 [ 184 | PositionSensor { 185 | name "hip2z_pos" 186 | } 187 | RotationalMotor { 188 | name "hip2z" 189 | maxTorque 100 190 | } 191 | ] 192 | endPoint Solid { 193 | translation -0.19999590437609466 -4.300726290571177e-05 0.09999999687727783 194 | rotation 0.5048173017587092 -0.8632262112824518 8.662308903962538e-10 3.734220292014386e-07 195 | children [ 196 | DEF KNEE2 HingeJoint { 197 | jointParameters HingeJointParameters { 198 | axis 0 0 1 199 | anchor 0 -0.15 0 200 | } 201 | device [ 202 | PositionSensor { 203 | name "knee2_pos" 204 | } 205 | RotationalMotor { 206 | name "knee2" 207 | maxTorque 100 208 | } 209 | ] 210 | endPoint Solid { 211 | translation 0 -0.15 0 212 | rotation 0 0 1 0 213 | children [ 214 | DEF KNEE2_GROUP Group { 215 | children [ 216 | DEF FOOT Transform { 217 | translation 0 -0.15 0 218 | rotation 1 0 0 1.5707 219 | children [ 220 | Shape { 221 | appearance PBRAppearance { 222 | baseColor 0.180392 0.203922 0.211765 223 | roughness 0.5 224 | metalness 0 225 | } 226 | geometry Cylinder { 227 | height 0.05 228 | radius 0.03 229 | } 230 | } 231 | ] 232 | } 233 | DEF SPRING Transform { 234 | translation 0 -0.075 0 235 | children [ 236 | Shape { 237 | appearance PBRAppearance { 238 | baseColor 0 0.8 0 239 | roughness 0.5 240 | } 241 | geometry Cylinder { 242 | height 0.15 243 | radius 0.015 244 | } 245 | } 246 | ] 247 | } 248 | DEF KNEE Transform { 249 | rotation 1 0 0 1.5707 250 | children [ 251 | Shape { 252 | appearance PBRAppearance { 253 | baseColor 0.8 0 0 254 | roughness 1 255 | } 256 | geometry Cylinder { 257 | height 0.03 258 | radius 0.03 259 | } 260 | } 261 | ] 262 | } 263 | ] 264 | } 265 | ] 266 | boundingObject USE KNEE2_GROUP 267 | physics Physics { 268 | } 269 | } 270 | } 271 | DEF HIP2_GROUP Group { 272 | children [ 273 | DEF FEMUR_TRANS Transform { 274 | translation 0 -0.075 0 275 | children [ 276 | Shape { 277 | appearance PBRAppearance { 278 | baseColor 0 0.8 0 279 | roughness 0.5 280 | } 281 | geometry Cylinder { 282 | height 0.15 283 | radius 0.015 284 | } 285 | } 286 | ] 287 | } 288 | DEF HIP2_TRANS Transform { 289 | children [ 290 | Shape { 291 | appearance PBRAppearance { 292 | baseColor 0.8 0 0 293 | roughness 0.5 294 | } 295 | geometry Sphere { 296 | radius 0.03 297 | } 298 | } 299 | ] 300 | } 301 | ] 302 | } 303 | ] 304 | name "hip2" 305 | boundingObject USE HIP2_GROUP 306 | physics Physics { 307 | } 308 | } 309 | } 310 | DEF HIP1 Hinge2Joint { 311 | jointParameters HingeJointParameters { 312 | anchor 0.2 0 -0.1 313 | } 314 | jointParameters2 JointParameters { 315 | } 316 | device [ 317 | PositionSensor { 318 | name "hip1x_pos" 319 | } 320 | RotationalMotor { 321 | name "hip1x" 322 | maxTorque 100 323 | } 324 | ] 325 | device2 [ 326 | PositionSensor { 327 | name "hip1z_pos" 328 | } 329 | RotationalMotor { 330 | name "hip1z" 331 | maxTorque 100 332 | } 333 | ] 334 | endPoint Solid { 335 | translation 0.20000228302022832 -1.9151218807147833e-05 -0.0999999994269053 336 | rotation 0.28194453531958596 -0.9594307056809381 -1.2344127834538342e-09 1.1151007970493858e-07 337 | children [ 338 | DEF KNEE1 HingeJoint { 339 | jointParameters HingeJointParameters { 340 | axis 0 0 1 341 | anchor 0 -0.15 0 342 | } 343 | device [ 344 | PositionSensor { 345 | name "knee1_pos" 346 | } 347 | RotationalMotor { 348 | name "knee1" 349 | maxTorque 100 350 | } 351 | ] 352 | endPoint Solid { 353 | translation 0 -0.15 0 354 | rotation 0 0 1 0 355 | children [ 356 | DEF KNEE1_GROUP Group { 357 | children [ 358 | DEF FOOT Transform { 359 | translation 0 -0.15 0 360 | rotation 1 0 0 1.5707 361 | children [ 362 | Shape { 363 | appearance PBRAppearance { 364 | baseColor 0.180392 0.203922 0.211765 365 | roughness 0.5 366 | metalness 0 367 | } 368 | geometry Cylinder { 369 | height 0.05 370 | radius 0.03 371 | } 372 | } 373 | ] 374 | } 375 | DEF SPRING Transform { 376 | translation 0 -0.075 0 377 | children [ 378 | Shape { 379 | appearance PBRAppearance { 380 | baseColor 0 0.8 0 381 | roughness 0.5 382 | } 383 | geometry Cylinder { 384 | height 0.15 385 | radius 0.015 386 | } 387 | } 388 | ] 389 | } 390 | DEF KNEE Transform { 391 | rotation 1 0 0 1.5707 392 | children [ 393 | Shape { 394 | appearance PBRAppearance { 395 | baseColor 0.8 0 0 396 | roughness 1 397 | } 398 | geometry Cylinder { 399 | height 0.03 400 | radius 0.03 401 | } 402 | } 403 | ] 404 | } 405 | ] 406 | } 407 | ] 408 | boundingObject USE KNEE1_GROUP 409 | physics Physics { 410 | } 411 | } 412 | } 413 | DEF HIP1_GROUP Group { 414 | children [ 415 | DEF FEMUR_TRANS Transform { 416 | translation 0 -0.075 0 417 | children [ 418 | Shape { 419 | appearance PBRAppearance { 420 | baseColor 0 0.8 0 421 | roughness 0.5 422 | } 423 | geometry Cylinder { 424 | height 0.15 425 | radius 0.015 426 | } 427 | } 428 | ] 429 | } 430 | DEF HIP1_TRANS Transform { 431 | children [ 432 | Shape { 433 | appearance PBRAppearance { 434 | baseColor 0.8 0 0 435 | roughness 0.5 436 | } 437 | geometry Sphere { 438 | radius 0.03 439 | } 440 | } 441 | ] 442 | } 443 | ] 444 | } 445 | ] 446 | name "hip1" 447 | boundingObject USE HIP1_GROUP 448 | physics Physics { 449 | } 450 | } 451 | } 452 | DEF HIP0 Hinge2Joint { 453 | jointParameters HingeJointParameters { 454 | anchor 0.2 0 0.1 455 | } 456 | jointParameters2 JointParameters { 457 | } 458 | device [ 459 | PositionSensor { 460 | name "hip0x_pos" 461 | } 462 | RotationalMotor { 463 | name "hip0x" 464 | maxTorque 100 465 | } 466 | ] 467 | device2 [ 468 | PositionSensor { 469 | name "hip0z_pos" 470 | } 471 | RotationalMotor { 472 | name "hip0z" 473 | maxTorque 100 474 | } 475 | ] 476 | endPoint Solid { 477 | translation 0.200004 -7.48718e-06 0.1 478 | rotation -0.6315110604808886 0.7753668683212505 -1.3688848147934803e-08 1.5769906706002898e-07 479 | children [ 480 | DEF KNEE0 HingeJoint { 481 | jointParameters HingeJointParameters { 482 | axis 0 0 1 483 | anchor 0 -0.15 0 484 | } 485 | device [ 486 | PositionSensor { 487 | name "knee0_pos" 488 | } 489 | RotationalMotor { 490 | name "knee0" 491 | maxTorque 100 492 | } 493 | ] 494 | endPoint Solid { 495 | translation 0 -0.15 0 496 | rotation 0 0 1 0 497 | children [ 498 | DEF KNEE0_GROUP Group { 499 | children [ 500 | DEF KNEE Transform { 501 | rotation 1 0 0 1.5707 502 | children [ 503 | DEF KNEE_SHAPE Shape { 504 | appearance PBRAppearance { 505 | baseColor 0.8 0 0 506 | roughness 0.5 507 | } 508 | geometry Cylinder { 509 | height 0.03 510 | radius 0.03 511 | } 512 | } 513 | ] 514 | } 515 | DEF FOOT0 Transform { 516 | translation 0 -0.15 0 517 | rotation 1 0 0 1.5707 518 | children [ 519 | Shape { 520 | appearance PBRAppearance { 521 | baseColor 0.180392 0.203922 0.211765 522 | roughness 0.5 523 | metalness 0.5 524 | } 525 | geometry Cylinder { 526 | height 0.05 527 | radius 0.03 528 | } 529 | } 530 | ] 531 | } 532 | DEF SPRING Transform { 533 | translation 0 -0.075 0 534 | rotation 0 0 1 0 535 | children [ 536 | Shape { 537 | appearance PBRAppearance { 538 | baseColor 0 0.8 0 539 | roughness 0.5 540 | } 541 | geometry Cylinder { 542 | height 0.15 543 | radius 0.015 544 | } 545 | } 546 | ] 547 | } 548 | ] 549 | } 550 | ] 551 | boundingObject USE KNEE0_GROUP 552 | physics Physics { 553 | } 554 | } 555 | } 556 | DEF HIP0_GROUP Group { 557 | children [ 558 | DEF FEMUR_TRANS Transform { 559 | translation 0 -0.075 0 560 | children [ 561 | Shape { 562 | appearance DEF LEG_APPEARANCE PBRAppearance { 563 | baseColor 0 0.8 0 564 | roughness 0.5 565 | } 566 | geometry Cylinder { 567 | height 0.15 568 | radius 0.015 569 | } 570 | } 571 | ] 572 | } 573 | DEF HIP_TRANS Transform { 574 | children [ 575 | DEF HIP_SHAPE Shape { 576 | appearance DEF JOINT_APPEARANCE PBRAppearance { 577 | baseColor 0.8 0 0 578 | roughness 0.5 579 | } 580 | geometry Sphere { 581 | radius 0.03 582 | } 583 | } 584 | ] 585 | } 586 | ] 587 | } 588 | ] 589 | name "hip0" 590 | boundingObject USE HIP0_GROUP 591 | physics Physics { 592 | centerOfMass [ 593 | 0 0 0 594 | ] 595 | } 596 | } 597 | } 598 | DEF BODY_SHAPE Shape { 599 | appearance DEF BODY_APPEARANCE PBRAppearance { 600 | baseColor 0.756863 0.490196 0.0666667 601 | roughness 0.5 602 | metalness 0.1 603 | } 604 | geometry Box { 605 | size 0.4 0.05 0.2 606 | } 607 | } 608 | ] 609 | name "dot_go" 610 | contactMaterial "dog" 611 | boundingObject USE BODY_SHAPE 612 | physics Physics { 613 | density 10 614 | } 615 | controller "quadruped_controller" 616 | } 617 | DEF WORKBANCH Solid { 618 | translation 0 -0.08 0 619 | children [ 620 | DEF WORKBANCH_GROUP Group { 621 | children [ 622 | DEF TOP Transform { 623 | translation 0 0.16 0 624 | children [ 625 | Shape { 626 | appearance PBRAppearance { 627 | baseColor 0.333333 0.341176 0.32549 628 | } 629 | geometry Box { 630 | size 0.1 0.02 0.4 631 | } 632 | } 633 | ] 634 | } 635 | DEF BOTTOM Transform { 636 | translation 0 -0.2 0 637 | children [ 638 | Shape { 639 | appearance PBRAppearance { 640 | baseColor 0.333333 0.341176 0.32549 641 | } 642 | geometry Box { 643 | size 0.5 0.05 0.5 644 | } 645 | } 646 | ] 647 | } 648 | DEF CYLINDER Shape { 649 | appearance PBRAppearance { 650 | baseColor 0.333333 0.341176 0.32549 651 | metalness 0 652 | } 653 | geometry Cylinder { 654 | height 0.34 655 | radius 0.06 656 | } 657 | } 658 | ] 659 | } 660 | ] 661 | boundingObject USE WORKBANCH_GROUP 662 | physics Physics { 663 | density 2000 664 | } 665 | } 666 | --------------------------------------------------------------------------------