diff --git a/Dog_connection_test.m b/Dog_connection_test.m index bbd1627..48453a7 100644 --- a/Dog_connection_test.m +++ b/Dog_connection_test.m @@ -4,7 +4,7 @@ %% Robot Dog Network Parameters % this IP is the vm ip -Robot_Dog_IP = '192.168.123.161'; +Robot_Dog_IP = '192.168.254.134'; Robot_Dog_Port = 1145; @@ -56,6 +56,3 @@ Control_Command(1)=12; Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); - - - diff --git a/Follow.m b/Follow.m index faf2583..1fecd96 100644 --- a/Follow.m +++ b/Follow.m @@ -5,7 +5,7 @@ %% Robot Dog Network Parameters % this IP is the vm ip -Robot_Dog_IP = '192.168.123.161'; +Robot_Dog_IP = '192.168.254.132'; Robot_Dog_Port = 1145; % Robot Dog Command Initialized @@ -15,7 +15,8 @@ % if isempty(gcp('nocreate')) % parpool; % end -% futureResult = parallel.FevalFuture; +% futureResult = +% parallel.FevalFuture; %% Feedback Control Parameters % Porportional constant on velocity action @@ -126,6 +127,7 @@ Dog_Speed_Record=[]; Dog_Pos_Record=[]; +Car_R=[] while true [init_time, x, z, yaw] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw]; Dog_Pos_Record=[0, x, z, yaw]; @@ -149,36 +151,36 @@ i=i+1; Dog_Pos_Record=[Dog_Pos_Record ; real_time, x, z, yaw]; Rotation_matrix = [cosd(yaw), -sind(yaw) ; sind(yaw),cosd(yaw) ]; - + Car_R=[Car_R;Target_Point,real_time]; d_dog_pos = Dog_Pos_Record(i,:)-Dog_Pos_Record(i-1,:); %[dtime, dx, dz, dyaw] - - + + Real_Dog_Speed_Room = [d_dog_pos(2)/d_dog_pos(1), d_dog_pos(3)/d_dog_pos(1)]; Real_Dog_Speed_Dog = Rotation_matrix*Real_Dog_Speed_Room'; - + Vector_D_T = Target_Point-[x z]; % Get vector - + Norm_VDT = norm(Vector_D_T); % Calculate Distance scale=Control_Speed/Norm_VDT; Ref_Speed_Room=scale*Vector_D_T; Ref_Speed_Dog = Rotation_matrix*Ref_Speed_Room'; Error_Speed_Dog = Ref_Speed_Dog-Real_Dog_Speed_Dog; - + %error yaw calculate [Error_Yaw,Mode2_Yaw_Ref]=Yaw_Controllor(yaw_set,yaw,Vector_D_T); %PID Control [Control_x,integral_x]=PID_Controllor(K_P_x,K_I_x,K_D_x,d_dog_pos(1),Error_Speed_Dog(1),integral_x,previous_error_x,p_x_limit,i_x_limit,d_x_limit); [Control_z,integral_z]=PID_Controllor(K_P_z,K_I_z,K_D_z,d_dog_pos(1),Error_Speed_Dog(2),integral_z,previous_error_z,p_z_limit,i_z_limit,d_z_limit); [Control_yaw,integral_yaw]=PID_Controllor(K_P_yaw,K_I_yaw,K_D_yaw,d_dog_pos(1),Error_Yaw,integral_yaw,previous_error_yaw,p_yaw_limit,i_yaw_limit,d_yaw_limit); - + previous_error_x = Error_Speed_Dog(1); previous_error_z = Error_Speed_Dog(2); previous_error_yaw = Error_Yaw; disp([Real_Dog_Speed_Dog',Error_Speed_Dog',Control_x,Control_z,Control_yaw]) - + Dog_Speed_Record=[Dog_Speed_Record;real_time,Real_Dog_Speed_Dog',Ref_Speed_Dog',Error_Speed_Dog',Control_x,Control_z,yaw,Mode2_Yaw_Ref,Control_yaw]; %[rtime, real_x_speed, real_z_speed, ref_x_speed, ref_z_speed, error_x, error_z] - + %async_robot_dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); if Norm_VDT> Switch_Distance Control_Command(10) = Control_x; %x @@ -188,9 +190,9 @@ Control_Command(10) = 0; %x Control_Command(9) = 0; %z Control_Command(11) = Control_yaw; %yaw - if Error_Yaw < 5 - Control_Command(11) = 0; - end + % if Error_Yaw < 5 + % Control_Command(11) = 0; + % end integral_z=0; integral_x=0; integral_yaw=0; @@ -198,43 +200,23 @@ previous_error_z=0; previous_error_x=0; end - + Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); end end %% Speed figures figure; -plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,2),'DisplayName','Measure Speed'); -title('Robot Dog X Speed') -hold on; -plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,4),'LineWidth',1,'DisplayName','Ref Speed'); -plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,6),'LineWidth',1,'DisplayName','Error'); -plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,8),'LineWidth',1,'DisplayName','Control'); -legend; -xlabel('Time(t)'); -ylabel('Speed(m/s)'); -hold off; - -figure; -plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,3),'DisplayName','Measure Speed'); -title('Robot Dog Z Speed') -hold on; -plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,5),'LineWidth',1,'DisplayName','Ref Speed'); -plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,7),'LineWidth',1,'DisplayName','Error'); -plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,9),'LineWidth',1,'DisplayName','Control'); -legend; -xlabel('Time(t)'); -ylabel('Speed(m/s)'); -hold off; - -figure; -plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,10),'DisplayName','Measure Yaw'); -title('Robot Dog Z Speed') +plot(Dog_Pos_Record(:,2),Dog_Pos_Record(:,3),'DisplayName','Dog'); +title('Robot Dog follow Car') +xlabel('X'); +ylabel('Z'); +set(gca,'XDir','reverse'); hold on; -plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,11),'LineWidth',1,'DisplayName','Yaw Ref'); -plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,12),'LineWidth',1,'DisplayName','Control_yaw'); +rectangle('Position',[-1.5,-1.5,3,3],'Curvature',[1,1]); +plot(Car_R(:,1),Car_R(:,2),'DisplayName','Car'); +xlim([-4,4]); +ylim([-2,2]); +daspect([1 1 1]); legend; -xlabel('Time(t)'); -ylabel('Speed(m/s)'); hold off; diff --git a/Get_Dog_Postion.m b/Get_Dog_Postion.m index 0aec8ca..8103217 100644 --- a/Get_Dog_Postion.m +++ b/Get_Dog_Postion.m @@ -25,10 +25,10 @@ if ~isempty(Dog_Pos) z=Dog_Pos.z; x=Dog_Pos.x; - + q = [Dog_Pos.qx, Dog_Pos.qy, Dog_Pos.qz, Dog_Pos.qw]; Eul_ang = quat2eul(q); -%% convert yaw + %% convert yaw if abs(Eul_ang(1)) > pi/2 if -Eul_ang(2) < 0 yaw = -Eul_ang(2)+2*pi; @@ -39,7 +39,7 @@ yaw = pi+Eul_ang(2); end yaw = yaw*180/pi; - + else time_stamp=0; diff --git a/Main.m b/Main.m index 8a27582..bd4b1dd 100644 --- a/Main.m +++ b/Main.m @@ -2,17 +2,9 @@ clc; close all; -%% Check List: -% 1.Power on Robot Dog -% 2.Connect Robot Dog Wifi: Unitree_Go393319A -% 3.Open Virtual Machine -% 4.Running /Desktop/unitree_matlab/build/udp_link press Enter to begin -% 5.Open Motive Check Broadcast and Network Interface -% 6.You are good for running - %% Robot Dog Network Parameters % this IP is the vm ip -Robot_Dog_IP = '192.168.123.161'; +Robot_Dog_IP = '192.168.254.134'; Robot_Dog_Port = 1145; %% Robot Dog Command Initialized @@ -25,17 +17,17 @@ % Porportional constant on velocity action K_P_x = 0.6; K_P_z = 0.6; -K_P_yaw = 0.07; +K_P_yaw = 0.01; % Integral K_I_x = 1.25; K_I_z = 1.25; -K_I_yaw = 0.01; +K_I_yaw = 0.002; % Derivative K_D_x = 0.08; K_D_z = 0.08; -K_D_yaw = 0.01; +K_D_yaw = 0.015; % limit propostional_x_limit = 0.7; % m/s @@ -53,13 +45,14 @@ % MODE % Mode 1: Dog will go to Target_point. % Mode 2: Dog will follow Way_Points. -% Mode 3: Dog will follow the path -Control_Mode=1; + +Control_Mode=2; % Target Point %[x,z] -Target_Point=[0 0]; +Target_Point=[-1 -1]; +yaw_set = -2; % YAW % [0,360) % -1: Disable yaw control @@ -76,7 +69,7 @@ % 180 % % wall computer wall -yaw_set = 0; + % THRESHOLD % Distance Threshold to switch to next way point @@ -91,21 +84,21 @@ % %% Path Planning -% % generate random points -% random_points_number = 4; -% random_points_r = 1.6; -% random_points_center_x = 0; -% random_points_center_z = 0; -% random_points = zeros(random_points_number,2); -% -% for i = 1:random_points_number -% angle = 2*pi*rand; -% random_r = random_points_r * sqrt(rand); -% random_point_x = random_points_center_x + random_r * cos(angle); -% random_point_z = random_points_center_z + random_r * sin(angle); -% random_points(i,:)=[random_point_x random_point_z]; -% end -% +% generate random points +random_points_number = 4; +random_points_r = 1.6; +random_points_center_x = 0; +random_points_center_z = 0; +random_points = zeros(random_points_number,2); + +for i = 1:random_points_number + angle = 2*pi*rand; + random_r = random_points_r * sqrt(rand); + random_point_x = random_points_center_x + random_r * cos(angle); + random_point_z = random_points_center_z + random_r * sin(angle); + random_points(i,:)=[random_point_x random_point_z]; +end + % figure; % viscircles([random_points_center_x, random_points_center_z], random_points_r,'LineStyle','--','Color','k'); % hold on; @@ -113,6 +106,10 @@ % axis equal; % hold off; +% figure; +% plot(Way_Points_x,Way_Points_z,'-o'); +% axis equal; + %% Instantiate client object to run Motive API commands @@ -177,55 +174,67 @@ breakflag = 0; Dog_Pos_Record=[]; -Dog_Pos_Record_Index = 1; -[time,x,z,yaw] = Get_Dog_Postion(theClient, Dog_ID); -init_time = time; +Dog_Speed = []; +Dog_Pos_Record_Index = 0; +stop_time = 0; +while true + [time,x,z,yaw] = Get_Dog_Postion(theClient, Dog_ID); + if time ~= 0 + init_time = time; + break + end +end -%% Main Loop while true % get position from camera [time,x,z,yaw] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw] Dog_Pos_Record=[Dog_Pos_Record; time, x, z, yaw, time-init_time]; Dog_Pos_Record_Index = Dog_Pos_Record_Index +1; + if Dog_Pos_Record_Index > 1 + Dog_Speed_D=Dog_Pos_Record(Dog_Pos_Record_Index,:)-Dog_Pos_Record(Dog_Pos_Record_Index-1,:); + Dog_Speed = [Dog_Speed;Dog_Speed_D(2)/Dog_Speed_D(1),Dog_Speed_D(3)/Dog_Speed_D(1),norm(Dog_Speed_D(2)/Dog_Speed_D(1),Dog_Speed_D(3)/Dog_Speed_D(1)),time-init_time]; + + end if Control_Mode == 2 Target_Point = [Way_Points_x(Way_Point_index) Way_Points_z(Way_Point_index)]; end - %% Feedback control - + % Feedback control + % Calculate Distance Point_Dog = [x z]; %[x,z] Vector_PD_TP = Target_Point-Point_Dog; % Get vector Norm_Vector = norm(Vector_PD_TP); % Calculate norm - + % rotation matrix to turn vector to robot dog frame Rotation_matrix = [cosd(yaw), -sind(yaw) ; sind(yaw),cosd(yaw) ]; - - if Control_Mode==1 || Norm_Vector > Distance_Threshold - %vector in robot dog frame = error_x error_z - Vector_rotated = Rotation_matrix*Vector_PD_TP'; - - Error_Yaw=Yaw_Controllor(yaw_set,yaw,Vector_PD_TP); - %% PID - Control_x=PID_Controllor(K_P_x,K_I_x,K_D_x,dt,Vector_rotated(1),integral_x,previous_error_x,propostional_x_limit,integral_x_limit,derivative_x_limit); - Control_z=PID_Controllor(K_P_z,K_I_z,K_D_z,dt,Vector_rotated(2),integral_z,previous_error_z,propostional_z_limit,integral_z_limit,derivative_z_limit); - Control_yaw=PID_Controllor(K_P_yaw,K_I_yaw,K_D_yaw,dt,Error_Yaw,integral_yaw,previous_error_yaw,propostional_yaw_limit,integral_yaw_limit,derivative_yaw_limit); - - propostional_x = Vector_rotated(1) * K_P_x; - propostional_z = Vector_rotated(2) * K_P_z; - propostional_yaw = Error_Yaw*pi/180 * K_P_yaw; - - %% set command - Control_Command(10) = Control_x; %x - Control_Command(9) = Control_z; %z - Control_Command(11) = Control_yaw; %yaw - else - % switch point - % clean integral -% integral_x = 0; -% integral_z = 0; -% integral_yaw = 0; - - if Control_Mode == 2 + + + %vector in robot dog frame = error_x error_z + Vector_rotated = Rotation_matrix*Vector_PD_TP'; + + Error_Yaw=Yaw_Controllor(yaw_set,yaw,Vector_PD_TP); + % PID + Control_x=PID_Controllor(K_P_x,K_I_x,K_D_x,dt,Vector_rotated(1),integral_x,previous_error_x,propostional_x_limit,integral_x_limit,derivative_x_limit); + Control_z=PID_Controllor(K_P_z,K_I_z,K_D_z,dt,Vector_rotated(2),integral_z,previous_error_z,propostional_z_limit,integral_z_limit,derivative_z_limit); + Control_yaw=PID_Controllor(K_P_yaw,K_I_yaw,K_D_yaw,dt,Error_Yaw,integral_yaw,previous_error_yaw,propostional_yaw_limit,integral_yaw_limit,derivative_yaw_limit); + + propostional_x = Vector_rotated(1) * K_P_x; + propostional_z = Vector_rotated(2) * K_P_z; + propostional_yaw = Error_Yaw*pi/180 * K_P_yaw; + + % set command + Control_Command(10) = Control_x; %x + Control_Command(9) = Control_z; %z + Control_Command(11) = Control_yaw; %yaw + if Norm_Vector < Distance_Threshold + if Control_Mode == 1 + if stop_time == 0 + stop_time = time + 5; + end + if time > stop_time + break + end + elseif Control_Mode == 2 % go to next way point if length(Way_Points_z)>Way_Point_index Way_Point_index=Way_Point_index+1; @@ -239,10 +248,11 @@ end end % print command - disp(Control_Command); + % disp(Control_Command); + % fprintf("hi\n"); % send command to vitrual machine Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); - %% draw figure + % draw figure plot(ax,circle_x,circle_y,'b-'); xlabel('X') ylabel('Z') @@ -260,30 +270,52 @@ ylim(ax,[-2,2]); hold off; drawnow; - %% Stop + % Stop if breakflag == 1 - figure; - subplot(2,1,1); - plot(Dog_Pos_Record(:,5),Dog_Pos_Record(:,2)); - xlabel('Time'); - ylabel('X'); - - subplot(2,1,2); - plot(Dog_Pos_Record(:,5),Dog_Pos_Record(:,3)); - xlabel('Time'); - ylabel('Z'); - - figure; - plot(Dog_Pos_Record(:,2),Dog_Pos_Record(:,3)); - xlabel('X'); - ylabel('Z'); - set(gca,'XDir','reverse'); - xlim([-3,3]); - ylim([-2,2]); - daspect([1 1 1]); break; end - %% time pause - pause(dt); + end +%% +figure; +subplot(2,1,1); +plot(Dog_Pos_Record(:,5),Dog_Pos_Record(:,2),'DisplayName','X'); +title('Robot Dog X') +yline(Target_Point(1),'DisplayName','Target X','Color','r') +legend; +xlabel('Time'); +ylabel('X'); + +subplot(2,1,2); +plot(Dog_Pos_Record(:,5),Dog_Pos_Record(:,3),'DisplayName','Z'); +title('Robot Dog Z') +yline(Target_Point(1),'DisplayName','Target Z','Color','r') +legend; +xlabel('Time'); +ylabel('Z'); + +figure; +plot(Dog_Pos_Record(:,5),Dog_Pos_Record(:,4),'DisplayName','Yaw'); +yline(yaw_set,'DisplayName','Target Yaw','Color','r') +legend; +xlabel('Time'); +ylabel('Yaw'); +title('Robot Dog Yaw') + +figure; +plot(Dog_Pos_Record(:,2),Dog_Pos_Record(:,3)); +xlabel('X'); +ylabel('Z'); +set(gca,'XDir','reverse'); +xlim([-3,3]); +ylim([-2,2]); +daspect([1 1 1]); + +figure; +plot(Dog_Speed(:,4),Dog_Speed(:,3)); +xlabel('Time'); +ylabel('Speed (m/s)'); +title('Robot Dog Norm Speed') + + diff --git a/Motive_test.m b/Motive_test.m index c5e3c1d..790ae7e 100644 --- a/Motive_test.m +++ b/Motive_test.m @@ -21,20 +21,25 @@ Dog_ID = 1; % Rigid body ID of the drone from Motive -%% initialized +%% initialized real_time=0; -run_time=1; +run_time=100; time=0; [init_time,x,z,yaw] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw] Dog_Pos_Record = [0,x,z,yaw]; i=1; +figure + %% Main Loop while true [time,x,z,yaw] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw] real_time=time-init_time; + time + real_time + init_time if ~isequal(Dog_Pos_Record(end,:),[real_time,x,z,yaw]) i=i+1; Dog_Pos_Record=[Dog_Pos_Record ; real_time,x,z,yaw]; @@ -43,16 +48,23 @@ break end end - + plot(Dog_Pos_Record(:,2),Dog_Pos_Record(:,3)) + xlabel('X'); + ylabel('Z'); + set(gca,'XDir','reverse'); + xlim([-3,3]); + ylim([-2,2]); + daspect([1 1 1]); + drawnow end %% figure -figure; -plot(Dog_Pos_Record(:,2),Dog_Pos_Record(:,3)); -xlabel('X'); -ylabel('Z'); -set(gca,'XDir','reverse'); -xlim([-3,3]); -ylim([-2,2]); -daspect([1 1 1]); +% figure; +% plot(Dog_Pos_Record(:,2),Dog_Pos_Record(:,3)); +% xlabel('X'); +% ylabel('Z'); +% set(gca,'XDir','reverse'); +% xlim([-3,3]); +% ylim([-2,2]); +% daspect([1 1 1]); diff --git a/PID_Controllor.m b/PID_Controllor.m index 69c9e66..3afa985 100644 --- a/PID_Controllor.m +++ b/PID_Controllor.m @@ -1,7 +1,7 @@ function [Control_Output,integral] = PID_Controllor(K_P,K_I,K_D,D_t,error,integral,previous_error,P_l,I_l,D_l) %UNTITLED Summary of this function goes here % Detailed explanation goes here -%Input: +%Input: % K_P,K_I,K_D, PID parameters % D_t time % error @@ -9,14 +9,14 @@ % previous_error % P_l,I_l,D_l PID output limit, set 0 to disable. -propostional = error * K_P; +propositional = error * K_P; integral = integral + K_I * error * D_t; derivative = K_D * (error - previous_error) /D_t; if P_l ~= 0 - if propostional > P_l - propostional = P_l; - elseif propostional < -P_l - propostional = -P_l; + if propositional > P_l + propositional = P_l; + elseif propositional < -P_l + propositional = -P_l; end end if I_l ~= 0 @@ -33,6 +33,6 @@ derivative = -D_l; end end -Control_Output = propostional+integral+derivative; +Control_Output = propositional+integral+derivative; end diff --git a/Robot_Dog.m b/Robot_Dog.m index bcb511e..d37f204 100644 --- a/Robot_Dog.m +++ b/Robot_Dog.m @@ -2,7 +2,7 @@ function Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Data) % Send command to robot dog % Create a UDP object -Robot_Dog_UDP = udpport +Robot_Dog_UDP = udpport("LocalHost",'192.168.254.1'); % Transform single float to uint8 Control_Data_Uint8 = typecast(Control_Data,'uint8'); % Send Command diff --git a/Yaw_Controllor.m b/Yaw_Controllor.m index 51dc6d2..7bcb418 100644 --- a/Yaw_Controllor.m +++ b/Yaw_Controllor.m @@ -14,6 +14,7 @@ % wall computer wall if yaw_set == -1 % not control yaw error_yaw_command=0; + yaw_ref = 0; elseif yaw_set == -2 % control yaw to motion direction angle_r = atan2(Vector(1),Vector(2)); yaw_set_mode2 = rad2deg(angle_r); diff --git a/instructions.md b/instructions.md new file mode 100644 index 0000000..b7d34bd --- /dev/null +++ b/instructions.md @@ -0,0 +1,110 @@ +# How to setup the Unitree Go1: HSL Edition + +## VM +1. create ubuntu vm (iso in drive) +2. network setup - 2 network interfaces: + - vmnet0 (bridged to intel dual band wireless) - lets you connect to dog, confirm that this is actually connected to wifi using edit > virtual network editor > change settings button at bottom right + - NAT - connect to internet + +3. make sure windows is actually connected to dog wifi, needs to be reconnected if dog is rebooted + +4. in ubuntu, sudo apt install: (these can all be installed in one line like "sudo apt install [package1] [package2] [package3] etc") + - net-tools + - git + - ssh (if you want to ssh into the vm) + - cmake + - g++ + - libboost-all-dev (this one might take a while) + +5. run ifconfig to ensure there are 2 network devices +6. make sure you can successfully ping 8.8.8.8 (or some other internet IP) and 192.168.123.161 (dog pi) + +7. git clone https://github.com/lyphix/unitree_matlab.git +8. cd into unitree_matlab +9. edit example/udp_link.cpp line 33 (if using vi, use ":set number" to show line numbers) +10. make sure the ip is "192.168.123.161" - dog pi IP +11. this might be set to 127.0.0.1 by default +12. cd back into unitree_matlab if you aren't already there +13. mkdir build +14. cd build +15. cmake .. +16. make + +17. if you make changes to any of the cpp files, you will need to run make again, but cmake does not need to be repeated unless you edit/change your Makefile, CMakeFiles, cmake_install.cmake, or CMakeCache.txt + +18. from the build directory, run ./example_walk to ensure the vm is connected to the dog +19. you'll be prompted to click Enter to start the program +20. if you don't want to run the full walking sequence, click ctrl+C + +## MOTIVE +1. open motive on windows +2. in the quick start menu, choose "Open Camera Calibration" +3. in the File Explorer window that appears, go to D:\OptitrackCalibrationFiles and open the newest calibration (CAL) file +4. you should be returned to the Motive window with a black grid plane, and if you zoom out, 12 cameras +5. place the dog at the center of the duct tape circle (there should be a duct tape X) and face it away from the computers (west) +6. ensure the legs are not underneath the dog, are folded, and with the feet facing the same direction as the dog itself +7. in Motive, find the 3 dots that represent the 3 reflective balls on top of the dog. click and drag to select them (and nothing else), then right click on them and select Rigid Body > Create From Selected Markers +you should now see a colored triangle with 3 colored balls at the edges, and a yellow ball at the center +8. in the left sidebar, under Assets (Live), check the Rigid Body number that corresponds to the dog. this should be Rigid Body 1 (if not, please remember the number as it will be important later) +9. to test, move the dog around and ensure that the rigid body in Motive moves along with it + +10. from the menu at the top of the Motive window, go to View > Data Streaming +11. a new set of options should open at the right side of Motive +12. select the IP address next to Local Interface (under Network Interface) and change it to Local Loopback +13. check the box to turn on Broadcast Frame Data in the OptiTrack Streaming Engine section + +## MATLAB +1. open matlab on windows +2. for these instructions, we will use Xi Luo's code from 2023, located at D:\xluo45\02MATLAB\High_Level_Control. if you wish to make edits or do your own experiments, please duplicate this folder to another location and use that +3. in matlab, open the High_Level_Control folder (or whatever directory you're working out of) +4. open Main.m +5. on line 7, change the Robot_Dog_IP variable: + - in ubuntu, run ifconfig. look at the network interface ens33, and find the IP address next to inet. it should resemble 192.168.254.133 + - set Robot_Dog_IP to this IP address (as a string, in quotes) +6. on line 49, ensure Control_Mode is set to 2. this will follow a series of waypoints (which are placed around the duct tape circle) +7. on line 55, change yaw_set as desired. if yaw_set is -1, the dog will simply "strafe" around the circle, facing the initial orientation the entire time. if yaw_set is -2, the dog will walk around the circle facing the direction it is moving, like a regular dog (technically) +8. on line 131, set Dog_ID to the Rigid Body number of the dog from Motive. by default, this should be 1 + +## RUN +1. in the vm, cd into the build directory, then run ./udp_link. when prompted, click Enter +2. in matlab, run Main.m (the green play button at the top of the screen) WARNING: the robot will now begin to move +3. matlab should open a new window showing the circle of target points, the dog's current position and direction, and (highlighted in red) the current point the dog is moving toward. When the dog reaches the red waypoint, the next waypoint on the circle will instead be highlighted so the dog can continue moving around the circle +4. as the dog follows the waypoints, a red line will be drawn to show the dog's path +5. when the dog has made its way around the circle, matlab will open 4 new plot windows to display the dog's x over time, z over time, yaw over time, path, and speed over time + +## TROUBLESHOOTING +there are 4 points of failure in this setup: motive to matlab, matlab to ubuntu, ubuntu to the dog, and the dog's control itself + +### motive to matlab +- in motive, ensure the 3 points of the dog are correctly created as a rigid body +- check that the rigid body number corresponding to the dog in motive is the same as the Dog_ID on line 131 of Main.m +- in motive, check that the broadcast frame data checkbox is checked (specifically the one under OptiTrack Streaming Engine) +- in motive, check that Local Interface under Network Interface under OptiTrack Streaming Engine is set to Local Loopback + +### matlab to ubuntu +- check that the Robot_Dog_IP on line 7 of Main.m is set to the inet address of ubuntu's network interface ens37 (run ifconfig in ubuntu to find this, make sure you can successfully ping this IP from windows) + - the IP address should resemble 192.168.254.X + - if it does not, then also change the IP address on line 5 of Robot_Dog.m to match the IPv4 address of Ethernet adapter VMware Network Adapter VMnet8 (which can be found by running ipconfig on windows) + - this should resemble X.X.X.1, where the first 3 numbers match the first 3 numbers of the ubuntu IP that was used +- ubuntu's firewall may block port 1145, which is used to communicate between Main.m and udp_link + - to unblock this, run sudo ufw allow 1145/udp on ubuntu +- the ControlCommand being sent to ubuntu by matlab should be of type 'single'. to do this, use the command "Control_Command = zeros(1,11,'single');" in matlab when instantiating Control_Command +- use udp_link's output to see what data it is receiving from matlab + +### ubuntu to dog +- check that windows is connected to wifi +- check that the two network adapters are set up correctly (VMnet0 and NAT) +- check that you can successfully ping 192.168.123.161 from ubuntu +- check that the IP on line 33 of udp_link.cpp is set to 192.168.123.161 + - run make from the build directory to ensure the udp_link executable is up to date +- check that example_walk works, to help isolate your issue + +### dog control +- check that yaw_set (Main.m line 55) is set as desired +- check that Control_Mode (Main.m line 49) is set as desired +- if the dog seems to be going haywire, check that its direction (depicted by an arrow in the Matlab plot) matches the direction it is physically facing + - the bottom of the plot window is the side of the wall with the computers (east) + - if this direction is incorrect, turn the dog to face the wall away from the computers (west). in motive, select Rigid Body 1 in the left sidebar under Assets (Live), then right-click it and select Settings. in the new Rigid Bodies pane, select the Orientation tab and click Reset To Current Orientation +- the dog's yaw value oscillates as it moves around the circle, so its path may not perfectly match the circle. unless it is going entirely in the wrong direction, give it a few seconds to see if it will self-correct + - if this becomes too much of a problem, it may be helpful to change yaw_set (Main.m line 55) to -1, and see if the dog can navigate the circle without yaw control +- if it seems like the dog is trying but wildly over- or undershooting the waypoints, your PID values may be off. in this case, you will need to do a lot more trial and error with the PID constants (Main.m lines 17 to 30) \ No newline at end of file diff --git a/speed_control.m b/speed_control.m index 8416aae..44092be 100644 --- a/speed_control.m +++ b/speed_control.m @@ -5,13 +5,15 @@ %% Robot Dog Network Parameters % this IP is the vm ip -Robot_Dog_IP = '192.168.123.161'; +Robot_Dog_IP = '192.168.254.132'; Robot_Dog_Port = 1145; % Robot Dog Command Initialized Control_Command = zeros(1,11,'single'); %velocity walking -Control_Command(1)=2; +Control_Command(1)=2; % mode +Control_Command(2)=2; % gaitType +Control_Command(4)=0.1; % footRaiseHeight % if isempty(gcp('nocreate')) % parpool; % end @@ -53,13 +55,13 @@ Control_Mode=2; -Control_Speed=0.8; +Control_Speed=0.65; -Switch_Distance=0.5; +Switch_Distance=0.3; % Target Point %[x,z] -Target_Point=[0 0]; +Target_Point=[1 1]; % YAW % [0,360) @@ -86,11 +88,27 @@ % Cricel way points Way_Points_center =[0,0]; -Way_Points_radius =1.5; -Way_Points_theta = linspace(0,2*pi,60); +Way_Points_radius =1.65; +Way_Points_theta = linspace(0,2*pi,120); Way_Points_x=Way_Points_center(1)+Way_Points_radius*cos(Way_Points_theta); Way_Points_z=Way_Points_center(2)+Way_Points_radius*sin(Way_Points_theta); +Way_Point_index = 1; +Target_Point = [Way_Points_x(Way_Point_index) + Way_Points_z(Way_Point_index)] + +PID_Controllor(Target_Point) + +if length(Way_points_z)>Way_Point_index + Way_Point_index = Way_Point_index+1; +else + %finished stop running + Control_Command(11) = 0; + Control_Command(10) = 0; + Control_Command(9) = 0; + breakflag = 1; +end + %% Instantiate client object to run Motive API commands % Check list: @@ -156,26 +174,26 @@ if Control_Mode == 2 Target_Point = [Way_Points_x(Way_Point_index) Way_Points_z(Way_Point_index)]; end -Dog_Speed_Record=zeros(1,14); +Dog_Speed_Record=zeros(1,15); %% Main Loop while true % get position from camera % async_robot_dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); [time, x, z, yaw] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw] real_time = time-init_time; - + if ~isequal(Dog_Pos_Record(end,:), [real_time, x, z, yaw]) %if not the same values i=i+1; Dog_Pos_Record=[Dog_Pos_Record ; real_time, x, z, yaw]; %Real_Time_Location_Figure(x,z,yaw,Target_Point(1),Target_Point(2),0.2,1.5,0,0); Rotation_matrix = [cosd(yaw), -sind(yaw) ; sind(yaw),cosd(yaw) ]; - + d_dog_pos = Dog_Pos_Record(i,:)-Dog_Pos_Record(i-1,:); %[dtime, dx, dz, dyaw] - + Real_Dog_Speed_Room = [d_dog_pos(2)/d_dog_pos(1), d_dog_pos(3)/d_dog_pos(1)]; Real_Dog_Speed_Dog = Rotation_matrix*Real_Dog_Speed_Room'; Last_Speed_Record=Dog_Speed_Record(end,13:14); - + %filter Dog_A=[(Real_Dog_Speed_Dog(1)-Last_Speed_Record(1))/d_dog_pos(1),(Real_Dog_Speed_Dog(2)-Last_Speed_Record(2))/d_dog_pos(1)]; Dog_A_R=[Dog_A_R;Dog_A]; @@ -186,7 +204,7 @@ else Filter_Dog_Speed_X=Real_Dog_Speed_Dog(1); end - + if Dog_A(2)>A_max Filter_Dog_Speed_Z=Last_Speed_Record(2)+A_max*d_dog_pos(1); elseif Dog_A(2)<-A_max @@ -194,16 +212,16 @@ else Filter_Dog_Speed_Z=Real_Dog_Speed_Dog(2); end - - + + Vector_D_T = Target_Point-[x z]; % Get vector - + Norm_VDT = norm(Vector_D_T); % Calculate Distance scale=Control_Speed/Norm_VDT; Ref_Speed_Room=scale*Vector_D_T; Ref_Speed_Dog = Rotation_matrix*Ref_Speed_Room'; Error_Speed_Dog = Ref_Speed_Dog-[Filter_Dog_Speed_X;Filter_Dog_Speed_Z]; - + %error yaw calculate [Error_Yaw,Mode2_Yaw_Ref]=Yaw_Controllor(yaw_set,yaw,Vector_D_T); %PID Control @@ -213,12 +231,12 @@ previous_error_x = Error_Speed_Dog(1); previous_error_z = Error_Speed_Dog(2); previous_error_yaw = Error_Yaw; - - %disp([Real_Dog_Speed_Dog',Error_Speed_Dog',Control_x,Control_z,Control_yaw]) - - Dog_Speed_Record=[Dog_Speed_Record;real_time,Real_Dog_Speed_Dog',Ref_Speed_Dog',Error_Speed_Dog',Control_x,Control_z,yaw,Mode2_Yaw_Ref,Control_yaw,Filter_Dog_Speed_X,Filter_Dog_Speed_Z]; + + % disp([Real_Dog_Speed_Dog',Error_Speed_Dog',Control_x,Control_z,Control_yaw]) + + Dog_Speed_Record=[Dog_Speed_Record;real_time,Real_Dog_Speed_Dog',Ref_Speed_Dog',Error_Speed_Dog',Control_x,Control_z,yaw,Mode2_Yaw_Ref,Control_yaw,Filter_Dog_Speed_X,Filter_Dog_Speed_Z,norm(Real_Dog_Speed_Dog)]; %[rtime, real_x_speed, real_z_speed, ref_x_speed, ref_z_speed, error_x, error_z] - + if Norm_VDT> Switch_Distance if yaw_set == -2 Control_Command(10) = Control_x*0.06; %x @@ -227,35 +245,49 @@ end Control_Command(9) = Control_z; %z Control_Command(11) = Control_yaw; %yaw + fprintf("1\n"); + % Control_Command = [2 2 0 0.1 0 0 0 0 0.4 0 2] + % Control_Command(9) = 0.4; + % Control_Command(10) = 0; + % Control_Command(11) = 2; + Control_Command else if Control_Mode == 1 Control_Command = zeros(1,11,'single'); %velocity walking - Control_Command(1)=2; + Control_Command(1)=2; % mode + Control_Command(2)=2; % gaitType + Control_Command(4)=0.1; % footRaiseHeight %async_robot_dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); + fprintf("2\n"); break; elseif Control_Mode == 2 if length(Way_Points_z)>Way_Point_index Way_Point_index=Way_Point_index+1; Target_Point = [Way_Points_x(Way_Point_index) Way_Points_z(Way_Point_index)]; + fprintf("3\n"); else Control_Command = zeros(1,11,'single'); %velocity walking - Control_Command(1)=2; + Control_Command(1)=2; % mode + Control_Command(2)=2; % gaitType + Control_Command(4)=0.1; % footRaiseHeight %async_robot_dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); + fprintf("4\n"); break; end end - + end Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); - + fprintf("5\n"); end end %% Speed figures figure; +subplot(2,1,1); plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,2),'DisplayName','Measure Speed'); title('Robot Dog X Speed') hold on; @@ -268,7 +300,7 @@ ylabel('Speed(m/s)'); hold off; -figure; +subplot(2,1,2); plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,3),'DisplayName','Measure Speed'); title('Robot Dog Z Speed') hold on; @@ -281,6 +313,20 @@ ylabel('Speed(m/s)'); hold off; +subplot(2,1,2); +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,3),'DisplayName','Measure Speed'); +title('Robot Dog Norm Speed') +hold on; +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,5),'LineWidth',1,'DisplayName','Ref Speed'); +plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,14),'LineWidth',1,'DisplayName','Filter'); +%plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,7),'LineWidth',1,'DisplayName','Error'); +%plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,9),'LineWidth',1,'DisplayName','Control'); +legend; +xlabel('Time(t)'); +ylabel('Speed(m/s)'); +hold off; + + figure; plot(Dog_Speed_Record(:,1),Dog_Speed_Record(:,10),'DisplayName','Measure Yaw'); title('Robot Dog Yaw') diff --git a/step_response.m b/step_response.m index db29b62..f55c284 100644 --- a/step_response.m +++ b/step_response.m @@ -17,7 +17,7 @@ Control_Command = zeros(1,11,'single'); %velocity walking Control_Command(1)=2; -Control_Command(2)=2; +Control_Command(2)=2; %1walk 2run Control_Command(9)=z_speed; Control_Command(10)=x_speed; % Robot dog command @@ -38,11 +38,6 @@ % % % wall computer wall - -if isempty(gcp('nocreate')) - parpool; -end -futureResult = parallel.FevalFuture; %% Instantiate client object to run Motive API commands % Check list: @@ -61,32 +56,34 @@ Dog_ID = 1; % Rigid body ID of the drone from Motive -%% initialized +%% initialized Dog_Pos_Record=[]; Dog_Speed_Record=[]; -i=0; +i=1; real_time=0; +while true + [time,x,z,yaw] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw] + if time ~0 + Dog_Pos_Record=[Dog_Pos_Record ; time,x,z,yaw]; + break + end +end %% Main Loop while true - async_robot_dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); - [Dog_Pos] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw] - if i<1 + Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); + [time,x,z,yaw] = Get_Dog_Postion(theClient, Dog_ID); %[time, x, z, yaw] + if ~isequal(Dog_Pos_Record(end,:),[time,x,z,yaw]) i=i+1; - Dog_Pos_Record=[Dog_Pos_Record ; Dog_Pos]; - else - if ~isequal(Dog_Pos_Record(end,:),Dog_Pos) - i=i+1; - Dog_Pos_Record=[Dog_Pos_Record ; Dog_Pos]; - d_dog_pos = Dog_Pos_Record(i,:)-Dog_Pos_Record(i-1,:); - real_time = Dog_Pos_Record(i-1,1)-Dog_Pos_Record(1,1); - speed_norm=norm(d_dog_pos(2:3))/d_dog_pos(1); - Dog_Speed_Record=[Dog_Speed_Record;real_time,speed_norm]; - if real_time>run_time - Control_Command = zeros(1,11,'single'); - async_robot_dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); - if speed_norm < 0.05 - break - end + Dog_Pos_Record=[Dog_Pos_Record ; [time,x,z,yaw]]; + d_dog_pos = Dog_Pos_Record(i,:)-Dog_Pos_Record(i-1,:); + real_time = Dog_Pos_Record(i-1,1)-Dog_Pos_Record(1,1); + speed_norm=norm(d_dog_pos(2:3))/d_dog_pos(1); + Dog_Speed_Record=[Dog_Speed_Record;real_time,speed_norm]; + if real_time>run_time + Control_Command = zeros(1,11,'single'); + Robot_Dog(Robot_Dog_IP,Robot_Dog_Port,Control_Command); + if speed_norm < 0.05 + break end end end