Robotics
/
IRDYn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feature/R1000-identification #2

Merged
cosmic_power merged 48 commits from feature/R1000-identification into main 2024-12-16 15:25:53 +00:00
Conversation 0 Commits 48 Files Changed 129 +23 -8
3 changed files with 23 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 379875828b - Show all commits

2
R1000_Dynamics_num.m
View File

@ -13,7 +13,7 @@ link_mass = robot.m;
com_pos = robot.com; com_pos = robot.com;
link_inertia = robot.I; link_inertia = robot.I;
thetalist = [zero_;zero_;q_J;q_J;zero_;zero_;zero_;zero_;zero_]'; thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
dthetalist = 1000*[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]'; dthetalist = 1000*[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]';
ddthetalist = 1000*[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]'; ddthetalist = 1000*[q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J;q_J]';

10
get_velocity.m
View File

@ -44,15 +44,11 @@ switch opt.KM_method
% FIXME: Need to get the velocity represent at base frame % FIXME: Need to get the velocity represent at base frame
robot.vel.w = V(1:3,2:end); robot.vel.w = V(1:3,2:end);
robot.vel.dw = Vd(1:3,2:end); robot.vel.dw = Vd(1:3,2:end);
% We need the vel at COM but we want to know vel at joint origin
for i = 1:robot.ndof for i = 1:robot.ndof
robot.vel.v(:,i) = V(4:6,i+1); robot.vel.v(:,i) = vec2skewSymMat(robot.Home.R(:,:,i)'*robot.com(:,i))*robot.vel.w(:,i)+V(4:6,i+1);
robot.vel.dv(:,i) = Vd(4:6,i+1) - vec2skewSymMat(robot.vel.dw(:,i))*robot.Home.R(:,:,i)'*robot.com(:,i)... robot.vel.dv(:,i) = vec2skewSymMat(robot.Home.R(:,:,i)'*robot.com(:,i))*robot.vel.dw(:,i)+Vd(4:6,i+1);
- vec2skewSymMat(robot.vel.w(:,i))*(vec2skewSymMat(robot.vel.w(:,i))*robot.Home.R(:,:,i)'*robot.com(:,i));
end end
% robot.vel.v = V(4:6,2:end);
% robot.vel.dv = Vd(4:6,2:end);
% robot.vel.dv=[zeros(2,robot.ndof);-robot.gravity(end)*ones(1,robot.ndof)];
case 'SDH' case 'SDH'
case 'MDH' case 'MDH'
switch opt.Vel_method switch opt.Vel_method

19
untitled3.asv Normal file
View File

@ -0,0 +1,19 @@
R = robot.kine.R;
P = robot.kine.t;
F1 = Adjoint(RpToTrans(RotX(pi/4),[1;2;3]))*robot.regressor.A(:,:,end)*robot.pi(:,end)
F2 = robot.regressor.A(:,:,end-1)*robot.pi(:,end-1)+F1
FF1 = Adjoint(TransInv(RpToTrans(RotX(pi/4),[1;2;3])))'*F_Simpack(end,:,1)'
FF2 = F_Simpack(end-1,:,1)'+FF1
%%
F1 = robot.regressor.A(:,:,end)*robot.pi(:,end);
F3 = robot.regressor.A(:,:,end-2)*robot.pi(:,end-2);
%%
F_Simpack(end,:,1)
F_Simpack(end-2,:,1)
%%
robot_pi_vecoter = reshape(robot.pi,[90,1]);
F = robot.regressor.U*robot_pi_vecoter;
FF = reshape(F,[6,9])
%%
F_Simpack(:,:,1)