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

match vel for regressor

This commit is contained in:
cosmic_power 2024-10-23 23:44:11 +08:00
parent 326d013579
commit 379875828b
3 changed files with 23 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
R1000_Dynamics_num.m
View File

@ -13,7 +13,7 @@ link_mass = robot.m;
com_pos = robot.com;
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]';
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
robot.vel.w = V(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
robot.vel.v(:,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)...
- vec2skewSymMat(robot.vel.w(:,i))*(vec2skewSymMat(robot.vel.w(:,i))*robot.Home.R(:,:,i)'*robot.com(:,i));
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) = vec2skewSymMat(robot.Home.R(:,:,i)'*robot.com(:,i))*robot.vel.dw(:,i)+Vd(4:6,i+1);
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 'MDH'
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)