feature/R1000-identification #2
|
|
@ -1,22 +0,0 @@
|
|||
close all;clc;clear
|
||||
file = [];
|
||||
opt.robot_def = 'direct';
|
||||
opt.KM_method = 'SCREW';
|
||||
opt.Vel_method = 'Direct';
|
||||
opt.LD_method = 'Direct';
|
||||
opt.debug = true;
|
||||
opt.robotName = 'R1000';
|
||||
opt.reGenerate = false;
|
||||
opt.Isreal = true;
|
||||
|
||||
robot = get_robot_R1000(file,opt);
|
||||
robot = get_Kinematics(robot, opt);
|
||||
|
||||
R1000_Dynamics_num;
|
||||
% R1000_Dynamics;
|
||||
% getGravityForce;
|
||||
robot = get_velocity(robot, opt);
|
||||
robot = get_regressor(robot,opt);
|
||||
% symbol matched
|
||||
% verify_regressor_R1000;
|
||||
% robot = get_baseParams(robot, opt);
|
||||
|
|
@ -1,180 +0,0 @@
|
|||
%% R1000
|
||||
N=9;
|
||||
% traj
|
||||
time = 0:0.01:1;
|
||||
f=1;
|
||||
q_J = sin(2*pi*f*time);
|
||||
qd_J = (2*pi*f)*cos(2*pi*f*time);
|
||||
qdd_J = -(2*pi*f)^2*sin(2*pi*f*time);
|
||||
zero_ = zeros(1,length(q_J));
|
||||
q_J = pi/4*ones(1,length(q_J));
|
||||
% Dynamics parameters
|
||||
link_mass = robot.m;
|
||||
com_pos = robot.com;
|
||||
link_inertia = robot.I;
|
||||
|
||||
thetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
dthetalist = [zero_;zero_;zero_;q_J;zero_;zero_;zero_;zero_;zero_]';
|
||||
ddthetalist = [zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_;zero_]';
|
||||
|
||||
% Get general mass matrix
|
||||
Glist=[];
|
||||
for i = 1:N
|
||||
link_inertia(:,:,i) = robot.Home.R(:,:,i)'*link_inertia(:,:,i)*robot.Home.R(:,:,i);
|
||||
Gb= [link_inertia(:,:,i),zeros(3,3);zeros(3,3),link_mass(i)*diag([1,1,1])];
|
||||
Glist = cat(3, Glist, Gb);
|
||||
end
|
||||
|
||||
% Get the com pos transformation in each joint reference frame
|
||||
% Mlist_CG = [];
|
||||
% for i = 0:N-1
|
||||
% if i == 0
|
||||
% M = robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
||||
% else
|
||||
% M = TransInv(transl(com_pos(:,i)))*robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
||||
% end
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
% end
|
||||
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
% Get the com pos transformation in each joint reference frame
|
||||
% FIXME: BUG here
|
||||
% Mlist_CG=[];
|
||||
% for i = 0:N-1
|
||||
% if i == 0
|
||||
% M=robot.T(:,:,i+1)*transl(com_pos(:,i+1));
|
||||
% else
|
||||
% rotation_i = diag([1,1,1]);
|
||||
% for j = 1:i
|
||||
% rotation_i = rotation_i*TransToRp(robot.T(:,:,i));
|
||||
% rotation_j = rotation_i*TransToRp(robot.T(:,:,i+1));
|
||||
% end
|
||||
% M = TransInv(RpToTrans(rotation_i,rotation_i*com_pos(:,i)))*robot.T(:,:,i+1)*RpToTrans(rotation_j,rotation_j*com_pos(:,i+1));
|
||||
% end
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
% end
|
||||
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
% ct=[];
|
||||
% Mlist_CG=[];
|
||||
% for i = 1:N
|
||||
% if i == 1
|
||||
% ct(:,i) = com_pos_R1(:,i);
|
||||
% elseif i< 9
|
||||
% ct(:,i) = -com_pos_R2(:,i-1)+com_pos_R1(:,i);
|
||||
% else
|
||||
% ct(:,i) = -com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
|
||||
% end
|
||||
% robot.Home.com(:,i) = ct(:,i);
|
||||
% M = RpToTrans(robot.T(1:3,1:3,i),robot.Home.R(:,:,i)*robot.Home.com(:,i));
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
% end
|
||||
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
% Mlist_CG=[];
|
||||
% for i = 1:N
|
||||
% if i == 1
|
||||
% M = [diag([1,1,1]),com_pos_R1(:,i);zeros(1,3),1];
|
||||
% elseif i<=8
|
||||
% M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,i)*(-com_pos_R2(:,i-1)+com_pos_R1(:,i)));
|
||||
% elseif i==9
|
||||
% M = RpToTrans(TransToRp(robot.T(:,:,i)),robot.Home.R(:,:,i)*(-[0;0;0.05896]+com_pos_R1(:,i-1)+com_pos_R1(:,i)));
|
||||
% end
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
% end
|
||||
% M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
% Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
% get the CG at the world base frame
|
||||
com_pos_R1 = robot.com_pos_R1;
|
||||
com_pos_R2 = robot.com_pos_R2;
|
||||
ct=[];
|
||||
Mlist_CG_Base=[];
|
||||
for i = 1:N
|
||||
if i == 1
|
||||
ct(:,i) = com_pos_R1(:,i);
|
||||
elseif i< 9
|
||||
ct(:,i) = ct(:,i-1)-com_pos_R2(:,i-1)+com_pos_R1(:,i);
|
||||
else
|
||||
ct(:,i) = ct(:,i-1)-com_pos_R1(:,i-1)-[0;0;0.05896]+com_pos_R1(:,i);
|
||||
end
|
||||
robot.Home.com(:,i) = ct(:,i);
|
||||
M_CG_Base = RpToTrans(robot.Home.R(:,:,i),robot.Home.com(:,i));
|
||||
Mlist_CG_Base = cat(3, Mlist_CG_Base, M_CG_Base);
|
||||
end
|
||||
% get the CG at the last GC frame
|
||||
Mlist_CG=[];
|
||||
for i = 1:N
|
||||
if i == 1
|
||||
Mlist_CG(:,:,i) = Mlist_CG_Base(:,:,i);
|
||||
else
|
||||
Mlist_CG(:,:,i) = TransInv(Mlist_CG_Base(:,:,i-1))*Mlist_CG_Base(:,:,i);
|
||||
end
|
||||
end
|
||||
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
Mlist_CG = cat(3, Mlist_CG, M);
|
||||
|
||||
|
||||
% Get the end efforce transformation in each joint reference frame
|
||||
Mlist_ED = [];
|
||||
for i = 1:N
|
||||
M = robot.T(:,:,i);
|
||||
Mlist_ED = cat(3, Mlist_ED, M);
|
||||
end
|
||||
M = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0]; [0, 0, 0, 1]];
|
||||
Mlist_ED = cat(3, Mlist_ED, M);
|
||||
|
||||
%TODO: Get Slist form DH table method
|
||||
% RRRRRRRRP
|
||||
Slist=robot.slist;
|
||||
|
||||
Vlinear=sym(zeros(3,3));
|
||||
J=sym(zeros(6,N));
|
||||
exf=[0;0;0;0;0;0];
|
||||
|
||||
for i = 1:length(q_J)
|
||||
[V1(:,:, i),Vd1(:,:, i),Adgab_mat(:,:,:,i),Fmat(:,:,i),taumat(:,i)] ...
|
||||
= InverseDynamics_debug(thetalist(i,:)', dthetalist(i,:)', ddthetalist(i,:)', ...
|
||||
[0;0;-9.806], exf, Mlist_CG, Glist, Slist);
|
||||
G(:,:,:,i) = FKinSpaceExpand(Mlist_CG, Slist, thetalist(i,:)');
|
||||
T(:,:,:,i)=FKinSpaceExpand(Mlist_ED, Slist, thetalist(i,:)');
|
||||
%Want to get the result from TC_delta, which means F at CG represent under frame at the last origin
|
||||
%why we need Mlist_ED
|
||||
%please explain this more
|
||||
F_Simpack(:,:,i) = getSimpackF(G(:,:,:,i),T(:,:,:,i),Mlist_ED,Fmat(:,:,i));
|
||||
end
|
||||
% plot Torque
|
||||
% above 2020b
|
||||
% F_Simpack = pagetranspose(F_Simpack);
|
||||
% below 2020b
|
||||
|
||||
for i = 1:3
|
||||
subplot(3,1,i);
|
||||
hold on;
|
||||
%added minus, so should be the same as simpack
|
||||
plot(time,taumat(i+6,:))
|
||||
xlabel('time(s)')
|
||||
ylabel('Torque(Nm)')
|
||||
% plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
|
||||
end
|
||||
|
||||
F_Simpack = permute(F_Simpack,[2 1 3]);
|
||||
figure(2)
|
||||
for i = 1:3
|
||||
subplot(3,1,i);
|
||||
hold on;
|
||||
%added minus, so should be the same as simpack
|
||||
plot(time,-reshape(F_Simpack(8,i,:),[1,length(F_Simpack)]))
|
||||
% plot(SPCK_Result.Crv(i+4).x,SPCK_Result.Crv(i+4).y)
|
||||
end
|
||||
|
||||
% Use Body Twist cal linear vel, but can't cal the end frame vel
|
||||
% [V2] = InverseDynamics_sym(thetalist, dthetalist, ddthetalist, ...
|
||||
% [0;0;0], exf, Mlist, Glist, Slist);
|
||||
% j=1;
|
||||
% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M12);
|
||||
% j=2;
|
||||
% Vlinear(:, j+1) = BodyVelToLinearVel(V2(:,j+1),G(:,:,j)*M23);
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<MATLABProject xmlns="http://www.mathworks.com/MATLABProjectFile" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" version="1.0"/>
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,29 @@
|
|||
function [rotInfo] = calculateArmRot(dynamicBasicInfo,theta)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 计算机器人各个轴臂的姿态，四元素/旋转矩阵...
|
||||
% dynamicBasicInfo: PSA机械臂的基本信息
|
||||
% setupInfo: SUA(setup arm)状态，各个轴的角度等 []
|
||||
% state:系统动力学的状态变量
|
||||
% 假设：关节变形和柔性体变形都是小量，忽略其对姿态的影响
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
rotInfo = struct;
|
||||
temp = struct;
|
||||
temp.rot = eye(3);
|
||||
temp.rotTran = eye(3);
|
||||
rotInfo.link = ([temp temp temp temp temp temp temp temp temp]);
|
||||
rotInfo.motor = ([temp temp temp temp temp temp temp temp temp]);
|
||||
rotInfo.link(1).rot = rot(dynamicBasicInfo.link(1).jointAxis,theta(1));
|
||||
rotInfo.link(1).rotTran = rotInfo.link(1).rot';
|
||||
|
||||
rotInfo.motor(1).rot = rotInfo.link(1).rot;
|
||||
rotInfo.motor(1).rotTran = rotInfo.motor(1).rot';
|
||||
for i=2:9
|
||||
if dynamicBasicInfo.link(i).jointAxis>3
|
||||
rotInfo.link(i).rot = rotInfo.link(i-1).rot*rot(dynamicBasicInfo.link(i).jointAxis-3,theta(i));
|
||||
else
|
||||
rotInfo.link(i).rot = rotInfo.link(i-1).rot;
|
||||
end
|
||||
rotInfo.link(i).rotTran = rotInfo.link(i).rot';
|
||||
rotInfo.motor(i).rot = rotInfo.link(i).rot;
|
||||
rotInfo.motor(i).rotTran = rotInfo.motor(i).rot';
|
||||
end
|
||||
|
|
@ -0,0 +1,49 @@
|
|||
function [jacoInfo] = calculateJacobian(robotRigidInfo,...
|
||||
rotInfo,...
|
||||
theta)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 计算机器人各个轴臂的雅可比矩阵，
|
||||
% dynamicBasicInfo: 机械臂的基本信息
|
||||
% rotInfo: 各个轴臂的姿态信息
|
||||
% theta: 转角
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
temp = struct;
|
||||
temp.rot = eye(3);
|
||||
temp.rotTran = eye(3);
|
||||
rotInfo.link = ([temp temp temp temp temp temp temp temp temp]);
|
||||
rotInfo.motor = ([temp temp temp temp temp temp temp temp temp]);
|
||||
for i=1:9
|
||||
jacoInfo.link(i).jaco = zeros(6,9);
|
||||
jacoInfo.motor(i).jaco = zeros(6,robotRigidInfo.dof);
|
||||
comToRotCenter = -rotInfo.link(i).rot*robotRigidInfo.link(i).R1;
|
||||
for j=i:-1:1
|
||||
if robotRigidInfo.link(j).jointAxis>3
|
||||
rotAxis=zeros(3,1);
|
||||
rotAxis(robotRigidInfo.link(j).jointAxis-3) = 1;
|
||||
% movement
|
||||
jacoInfo.link(i).jaco(1:3,j) = -vector2so3(comToRotCenter)*rotInfo.link(j).rot*rotAxis;
|
||||
if j>1
|
||||
comToRotCenter = comToRotCenter + ...
|
||||
rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R2 ...
|
||||
- rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R1;
|
||||
end
|
||||
% rotation
|
||||
jacoInfo.link(i).jaco(4:6,j) = rotInfo.link(i).rotTran*rotInfo.link(j).rot*rotAxis;
|
||||
else
|
||||
tranAxis=zeros(3,1);
|
||||
tranAxis(robotRigidInfo.link(j).jointAxis) = 1;
|
||||
% movement
|
||||
jacoInfo.link(i).jaco(1:3,j) = rotInfo.link(j).rot*tranAxis;
|
||||
if j>1
|
||||
comToRotCenter = comToRotCenter + ...
|
||||
rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R2 ...
|
||||
- rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R1 + ...
|
||||
rotInfo.link(j).rot*tranAxis*theta(j);
|
||||
end
|
||||
end
|
||||
motorRotAxis = zeros(3,1);
|
||||
motorRotAxis(robotRigidInfo.motor(i).jointAxis-3) = 1;
|
||||
jacoInfo.motor(i).jaco = jacoInfo.link(i).jaco;
|
||||
jacoInfo.motor(i).jaco(4:6,i) = robotRigidInfo.motor(i).gearRatio*motorRotAxis;
|
||||
end
|
||||
end
|
||||
|
|
@ -0,0 +1,50 @@
|
|||
function [jacoInfo] = calculateJacobian(robotRigidInfo,...
|
||||
rotInfo,...
|
||||
theta)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 计算机器人各个轴臂的雅可比矩阵，
|
||||
% dynamicBasicInfo: 机械臂的基本信息
|
||||
% rotInfo: 各个轴臂的姿态信息
|
||||
% theta: 转角
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
temp = struct;
|
||||
temp.jaco = zeros(6,9);
|
||||
jacoInfo = struct;
|
||||
jacoInfo.link = ([temp temp temp temp temp temp temp temp temp]);
|
||||
jacoInfo.motor = ([temp temp temp temp temp temp temp temp temp]);
|
||||
for i=1:9
|
||||
jacoInfo.link(i).jaco = zeros(6,9);
|
||||
jacoInfo.motor(i).jaco = zeros(6,9);
|
||||
comToRotCenter = -rotInfo.link(i).rot*robotRigidInfo.link(i).R1;
|
||||
for j=i:-1:1
|
||||
if robotRigidInfo.link(j).jointAxis>3
|
||||
rotAxis=zeros(3,1);
|
||||
rotAxis(robotRigidInfo.link(j).jointAxis-3) = 1;
|
||||
% movement
|
||||
jacoInfo.link(i).jaco(1:3,j) = -vector2so3(comToRotCenter)*rotInfo.link(j).rot*rotAxis;
|
||||
if j>1
|
||||
comToRotCenter = comToRotCenter + ...
|
||||
rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R2 ...
|
||||
- rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R1;
|
||||
end
|
||||
% rotation
|
||||
jacoInfo.link(i).jaco(4:6,j) = rotInfo.link(i).rotTran*rotInfo.link(j).rot*rotAxis;
|
||||
else
|
||||
tranAxis=zeros(3,1);
|
||||
tranAxis(robotRigidInfo.link(j).jointAxis) = 1;
|
||||
% movement
|
||||
jacoInfo.link(i).jaco(1:3,j) = rotInfo.link(j).rot*tranAxis;
|
||||
if j>1
|
||||
comToRotCenter = comToRotCenter + ...
|
||||
rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R2 ...
|
||||
- rotInfo.link(j-1).rot*robotRigidInfo.link(j-1).R1 + ...
|
||||
rotInfo.link(j).rot*tranAxis*theta(j);
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
motorRotAxis = zeros(3,1);
|
||||
motorRotAxis(robotRigidInfo.motor(i).jointAxis-3) = 1;
|
||||
jacoInfo.motor(i).jaco = jacoInfo.link(i).jaco;
|
||||
jacoInfo.motor(i).jaco(4:6,i) = robotRigidInfo.motor(i).gearRatio*motorRotAxis;
|
||||
end
|
||||
|
|
@ -0,0 +1,28 @@
|
|||
function [massMatrix,totalMass,gravityForce] = calculateMassMatrix(dynamicBasicInfo,...
|
||||
toolMass,jacoInfo)
|
||||
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% 计算机器人系统的质量矩阵，
|
||||
% dynamicBasicInfo: 机械臂的基本信息
|
||||
% jacoInfo: 雅可比矩阵
|
||||
% toolMass: 器械的质量
|
||||
%
|
||||
% author:li.jiang
|
||||
% e-mail:li.jiang@csrbtx.com
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
g = -9.8;
|
||||
massMatrix = zeros(9,9); % init massMatrix to be zeros
|
||||
totalMass = 0;
|
||||
gravityForce = zeros(9,1);
|
||||
for i=1:9-1
|
||||
tempMatrix = [dynamicBasicInfo.link(i).mass*eye(3) zeros(3,3);
|
||||
zeros(3,3) dynamicBasicInfo.link(i).inertialMatrix];
|
||||
massMatrix = massMatrix + jacoInfo.link(i).jaco'*tempMatrix*jacoInfo.link(i).jaco;
|
||||
totalMass = totalMass+dynamicBasicInfo.link(i).mass;
|
||||
gravityForce = gravityForce + jacoInfo.link(i).jaco'*[0;0;g*dynamicBasicInfo.link(i).mass;0;0;0];
|
||||
end
|
||||
tempMatrix = [(dynamicBasicInfo.link(9).mass+toolMass)*eye(3) zeros(3,3);
|
||||
zeros(3,3) dynamicBasicInfo.link(9).inertialMatrix];
|
||||
massMatrix = massMatrix + jacoInfo.link(9).jaco'*tempMatrix*jacoInfo.link(9).jaco;
|
||||
totalMass = totalMass+dynamicBasicInfo.link(9).mass+toolMass;
|
||||
gravityForce = gravityForce + jacoInfo.link(9).jaco'*[0;0;g*(dynamicBasicInfo.link(9).mass+toolMass);0;0;0];
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
function [dynamicBasicInfo] = get_robot_arm_dynamic_basic_info()
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% get_PSA_robot_arm_dynamic_basic_info.m
|
||||
% robot arm dynamic init file,load dynamic basic info into a struct
|
||||
% author:Li Jiang
|
||||
% e-mail:li.jiang@csrbtx.com
|
||||
%
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
%% load data from files
|
||||
dynamicBasicInfo = get_robot_arm_rigid_info();
|
||||
dynamicBasicInfo.dof = 9;
|
||||
|
|
@ -0,0 +1,155 @@
|
|||
function [rigidInfo] = get_robot_arm_rigid_info()
|
||||
rigidInfo.link(1).mass = 2.1315260e+01;
|
||||
rigidInfo.link(1).jointAxis = 6;
|
||||
rigidInfo.link(1).R1 = -[ 1.0440117e+02 -1.1125559e+01 1.2822933e+02]'*1e-3;
|
||||
rigidInfo.link(1).R2 = -[-8.0598830e+01 -4.2225559e+01 -1.5870672e+01]'*1e-3;
|
||||
rigidInfo.link(1).inertialMatrix = [1.2998819e+08 1.2600723e+07 -3.5301328e+07;
|
||||
1.2600723e+07 2.8872462e+08 3.7145910e+06;
|
||||
-3.5301328e+07 3.7145910e+06 2.4658035e+08]*1e-9;
|
||||
rigidInfo.motor(1).mass = 0;
|
||||
rigidInfo.motor(1).jointAxis = 6;
|
||||
rigidInfo.motor(1).gearRatio = 50;
|
||||
rigidInfo.motor(1).R1 = [0 0 0];
|
||||
rigidInfo.motor(1).inertialMatrix = [0 0 0;
|
||||
0 0 0;
|
||||
0 0 0]*1e-9;
|
||||
rigidInfo.motor(1).inertialMatrix(rigidInfo.motor(1).jointAxis-3,...
|
||||
rigidInfo.motor(1).jointAxis-3) = 101219.53*1e-9*0;
|
||||
|
||||
|
||||
rigidInfo.link(2).mass = 1.9235354e+01;
|
||||
rigidInfo.link(2).jointAxis = 5;
|
||||
rigidInfo.link(2).R1 = -[ 3.5562148e+02 1.2378198e+01 1.8395830e-03]'*1e-3;
|
||||
rigidInfo.link(2).R2 = -[-2.4437852e+02 9.4778198e+01 1.8395830e-03]'*1e-3;
|
||||
rigidInfo.link(2).inertialMatrix = [9.4072398e+07 1.1819105e+08 -8.6335749e+03;
|
||||
1.1819105e+08 1.2626927e+09 -2.3575826e+03;
|
||||
-8.6335749e+03 -2.3575826e+03 1.2707334e+09]*1e-9;
|
||||
rigidInfo.motor(2).mass = 0;
|
||||
rigidInfo.motor(2).jointAxis = 5;
|
||||
rigidInfo.motor(2).gearRatio = 50;
|
||||
rigidInfo.motor(2).R1 = [0 0 0];
|
||||
rigidInfo.motor(2).inertialMatrix = [0 0 0;
|
||||
0 0 0;
|
||||
0 0 0]*1e-9;
|
||||
rigidInfo.motor(2).inertialMatrix(rigidInfo.motor(2).jointAxis-3,...
|
||||
rigidInfo.motor(2).jointAxis-3) = 101219.53*1e-9*0;
|
||||
|
||||
rigidInfo.link(3).mass = 1.0463871e+01;
|
||||
rigidInfo.link(3).jointAxis = 5;
|
||||
rigidInfo.link(3).R1 = -[2.4798242e+02 -3.5267916e+01 4.8648267e-02]'*1e-3;
|
||||
rigidInfo.link(3).R2 = -[-3.5201758e+02 -2.9007916e+01 4.8648266e-02]'*1e-3;
|
||||
rigidInfo.link(3).inertialMatrix = [2.7873904e+07 4.4214569e+06 1.5506243e+05;
|
||||
4.4214569e+06 5.4509917e+08 -2.2695805e+04;
|
||||
1.5506243e+05 -2.2695805e+04 5.3638465e+08]*1e-9;
|
||||
rigidInfo.motor(3).mass = 0;
|
||||
rigidInfo.motor(3).jointAxis = 5;
|
||||
rigidInfo.motor(3).gearRatio = 50;
|
||||
rigidInfo.motor(3).R1 = [0 0 0];
|
||||
rigidInfo.motor(3).inertialMatrix = [0 0 0;
|
||||
0 0 0;
|
||||
0 0 0]*1e-9;
|
||||
rigidInfo.motor(3).inertialMatrix(rigidInfo.motor(3).jointAxis-3,...
|
||||
rigidInfo.motor(3).jointAxis-3) = 101219.53*1e-9*0;
|
||||
|
||||
rigidInfo.link(4).mass = 6.1538806;
|
||||
rigidInfo.link(4).jointAxis = 5;
|
||||
rigidInfo.link(4).R1 = -[ 4.2341069e+01 5.2603527e+01 1.2271365e+01]'*1e-3;
|
||||
rigidInfo.link(4).R2 = -[-7.7658931e+01 -1.2956223e+01 1.9271365e+01]'*1e-3;
|
||||
rigidInfo.link(4).inertialMatrix = [1.3401032e+07 -3.3450696e+06 -4.9938448e+06;
|
||||
-3.3450696e+06 2.9613188e+07 -9.7003445e+05;
|
||||
-4.9938448e+06 -9.7003445e+05 2.9945110e+07]*1e-9;
|
||||
rigidInfo.motor(4).mass = 0;
|
||||
rigidInfo.motor(4).jointAxis = 5;
|
||||
rigidInfo.motor(4).gearRatio = 50;
|
||||
rigidInfo.motor(4).R1 = [0 0 0];
|
||||
rigidInfo.motor(4).inertialMatrix = [0 0 0;
|
||||
0 0 0;
|
||||
0 0 0]*1e-9;
|
||||
rigidInfo.motor(4).inertialMatrix(rigidInfo.motor(4).jointAxis-3,...
|
||||
rigidInfo.motor(4).jointAxis-3) = 101219.53*1e-9*0;
|
||||
|
||||
|
||||
rigidInfo.link(5).mass = 3.0882893;
|
||||
rigidInfo.link(5).jointAxis = 6;
|
||||
rigidInfo.link(5).R1 = -[1.0177553e+02 -3.4868152e-02 1.5686100e+00]'*1e-3;
|
||||
rigidInfo.link(5).R2 = -[-1.0372447e+02 -3.9239008e-02 -5.4313602e+00]'*1e-3;
|
||||
rigidInfo.link(5).inertialMatrix = [3.5376413e+06 -1.2573168e+02 -1.7221434e+06;
|
||||
-1.2573168e+02 1.7872508e+07 7.0524077e+02;
|
||||
-1.7221434e+06 7.0524077e+02 1.8213619e+07]*1e-9;
|
||||
rigidInfo.motor(5).mass = 0;
|
||||
rigidInfo.motor(5).jointAxis = 6;
|
||||
rigidInfo.motor(5).gearRatio = 50;
|
||||
rigidInfo.motor(5).R1 = [0 0 0];
|
||||
rigidInfo.motor(5).inertialMatrix = [0 0 0;
|
||||
0 0 0;
|
||||
0 0 0]*1e-9;
|
||||
rigidInfo.motor(5).inertialMatrix(rigidInfo.motor(5).jointAxis-3,...
|
||||
rigidInfo.motor(5).jointAxis-3) = 101219.53*1e-9*0;
|
||||
|
||||
rigidInfo.link(6).mass = 2.4450941;
|
||||
rigidInfo.link(6).jointAxis = 4;
|
||||
rigidInfo.link(6).R1 = -[5.9942347e+01 -4.8346177e-02 5.5784018e+00]'*1e-3;
|
||||
rigidInfo.link(6).R2 = -[-2.3757653e+01 -4.8346177e-02 4.0028402e+01]'*1e-3;
|
||||
rigidInfo.link(6).inertialMatrix = [3.7434848e+06 2.7091091e+03 -3.2187006e+05;
|
||||
2.7091091e+03 6.1710692e+06 4.3115007e+03;
|
||||
-3.2187006e+05 4.3115007e+03 5.2814895e+06]*1e-9;
|
||||
rigidInfo.motor(6).mass = 0;
|
||||
rigidInfo.motor(6).jointAxis = 4;
|
||||
rigidInfo.motor(6).gearRatio = 50;
|
||||
rigidInfo.motor(6).R1 = [0 0 0];
|
||||
rigidInfo.motor(6).inertialMatrix = [0 0 0;
|
||||
0 0 0;
|
||||
0 0 0]*1e-9;
|
||||
rigidInfo.motor(6).inertialMatrix(rigidInfo.motor(6).jointAxis-3,...
|
||||
rigidInfo.motor(6).jointAxis-3) = 101219.53*1e-9*0;
|
||||
|
||||
rigidInfo.link(7).mass = 3.6376589;
|
||||
rigidInfo.link(7).jointAxis = 6;
|
||||
rigidInfo.link(7).R1 = -[4.9494392e-02 -2.4240394e+00 -1.0332489e+02]'*1e-3;
|
||||
rigidInfo.link(7).R2 = -[4.9494392e-02 -2.4240394e+00 2.7222511e+02]'*1e-3;
|
||||
rigidInfo.link(7).inertialMatrix = [3.6676807e+07 1.9814032e+03 6.3921710e+03;
|
||||
1.9814032e+03 3.5426529e+07 -1.1636151e+05;
|
||||
6.3921710e+03 -1.1636151e+05 3.9665088e+06]*1e-9;
|
||||
rigidInfo.motor(7).mass = 0;
|
||||
rigidInfo.motor(7).jointAxis = 6;
|
||||
rigidInfo.motor(7).gearRatio = 50;
|
||||
rigidInfo.motor(7).R1 = [0 0 0];
|
||||
rigidInfo.motor(7).inertialMatrix = [0 0 0;
|
||||
0 0 0;
|
||||
0 0 0]*1e-9;
|
||||
rigidInfo.motor(7).inertialMatrix(rigidInfo.motor(7).jointAxis-3,...
|
||||
rigidInfo.motor(7).jointAxis-3) = 101219.53*1e-9*0;
|
||||
|
||||
rigidInfo.link(8).mass = 1.7005420;
|
||||
rigidInfo.link(8).jointAxis = 5;
|
||||
rigidInfo.link(8).R1 = -[-2.7158498e+01 -3.3865875e-01 -4.6588829e+01]'*1e-3;
|
||||
rigidInfo.link(8).R2 = -[ 2.2423433e+02 -3.3865729e-01 1.9671171e+01]'*1e-3;
|
||||
rigidInfo.link(8).inertialMatrix = [1.3876431e+06 -2.9158636e+04 3.8674359e+06;
|
||||
-2.9158636e+04 6.0220640e+07 1.7266885e+03;
|
||||
3.8674359e+06 1.7266885e+03 5.9261473e+07]*1e-9;
|
||||
rigidInfo.motor(8).mass = 0;
|
||||
rigidInfo.motor(8).jointAxis = 5;
|
||||
rigidInfo.motor(8).gearRatio = 50;
|
||||
rigidInfo.motor(8).R1 = [0 0 0];
|
||||
rigidInfo.motor(8).inertialMatrix = [0 0 0;
|
||||
0 0 0;
|
||||
0 0 0]*1e-9;
|
||||
rigidInfo.motor(8).inertialMatrix(rigidInfo.motor(8).jointAxis-3,...
|
||||
rigidInfo.motor(8).jointAxis-3) = 101219.53*1e-9*0;
|
||||
|
||||
rigidInfo.link(9).mass = 9.1507819e-1;
|
||||
rigidInfo.link(9).jointAxis = 1;
|
||||
rigidInfo.link(9).R1 = -[-1.9853197e+01 1.9076829e-01 -3.0075564e+01]'*1e-3;
|
||||
rigidInfo.link(9).R2 = -[-1.9853197e+01 1.9076829e-01 -3.0075564e+01]'*1e-3;
|
||||
rigidInfo.link(9).inertialMatrix = [8.3261779e+05 2.0399642e+03 1.3660323e+05;
|
||||
2.0399642e+03 1.6688880e+06 6.0504274e+03;
|
||||
1.3660323e+05 6.0504274e+03 1.3114690e+06]*1e-9;
|
||||
rigidInfo.motor(9).mass = 0;
|
||||
rigidInfo.motor(9).jointAxis = 4;
|
||||
rigidInfo.motor(9).gearRatio = 83.3;
|
||||
rigidInfo.motor(9).R1 = [0 0 0];
|
||||
rigidInfo.motor(9).inertialMatrix = [0 0 0;
|
||||
0 0 0;
|
||||
0 0 0]*1e-9;
|
||||
rigidInfo.motor(9).inertialMatrix(rigidInfo.motor(8).jointAxis-3,...
|
||||
rigidInfo.motor(8).jointAxis-3) = 101219.53*1e-9*0;
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% test mass calculation function
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% dynamicBasicInfo = get_PSA_robot_arm_dynamic_basic_info();
|
||||
dynamicBasicInfo = get_robot_arm_dynamic_basic_info();
|
||||
theta = zeros(9,1);
|
||||
% theta = [0;-pi/3;2*pi/3;-pi/3;pi/2;-pi/2;0;0;0];
|
||||
rotInfo = calculateArmRot(dynamicBasicInfo,theta);
|
||||
jaco = calculateJacobian(dynamicBasicInfo,...
|
||||
rotInfo,...
|
||||
theta);
|
||||
[massMatrix,~,gravityForce] = calculateMassMatrix(dynamicBasicInfo,...
|
||||
0,...
|
||||
jaco);
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info MetadataType="distributed" />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info Name="r1000" />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info ReadOnly="1" SingleValued="1" DataType="None" Name="分类" />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info ReadOnly="READ_ONLY" Name="工件" />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info ReadOnly="READ_ONLY" Name="便利" />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info ReadOnly="READ_ONLY" Name="派生" />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info ReadOnly="READ_ONLY" Name="设计" />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info ReadOnly="READ_ONLY" Name="无" />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info ReadOnly="READ_ONLY" Name="其他" />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info ReadOnly="READ_ONLY" Name="测试" />
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design" />
|
||||
</Category>
|
||||
</Info>
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design" />
|
||||
</Category>
|
||||
</Info>
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design" />
|
||||
</Category>
|
||||
</Info>
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design" />
|
||||
</Category>
|
||||
</Info>
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design" />
|
||||
</Category>
|
||||
</Info>
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="derived" />
|
||||
</Category>
|
||||
</Info>
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design" />
|
||||
</Category>
|
||||
</Info>
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design" />
|
||||
</Category>
|
||||
</Info>
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design" />
|
||||
</Category>
|
||||
</Info>
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design" />
|
||||
</Category>
|
||||
</Info>
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info Ref="code_generation" Type="Relative" />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info Ref="r1000_asm_1109_urdf/urdf" Type="Relative" />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info Ref="r1000_dynamic_analysis" Type="Relative" />
|
||||
|
|
@ -0,0 +1,2 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<Info />
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
function rotMatrix = rot(axis,angle)
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% return a rotation matrix from a rotation around axis about angle
|
||||
% axis: 1-x,2-y,3-z
|
||||
% angle:
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
switch axis
|
||||
case 1
|
||||
rotMatrix = [1 0 0;
|
||||
0 cos(angle) -sin(angle);
|
||||
0 sin(angle) cos(angle)];
|
||||
case 2
|
||||
rotMatrix = [cos(angle) 0 sin(angle);
|
||||
0 1 0;
|
||||
-sin(angle) 0 cos(angle)];
|
||||
case 3
|
||||
rotMatrix = [cos(angle) -sin(angle) 0;
|
||||
sin(angle) cos(angle) 0;
|
||||
0 0 1];
|
||||
otherwise
|
||||
rotMatrix = eye(3);
|
||||
end
|
||||
|
|
@ -0,0 +1,10 @@
|
|||
function so3 = vector2so3(vector)
|
||||
so3 = zeros(3,3);
|
||||
so3(1,2) = -vector(3);
|
||||
so3(1,3) = vector(2);
|
||||
|
||||
so3(2,1) = vector(3);
|
||||
so3(2,3) = -vector(1);
|
||||
|
||||
so3(3,1) = -vector(2);
|
||||
so3(3,2) = vector(1);
|
||||
Loading…
Reference in New Issue