Hello. I want to implement the UR10e Force control using InverseDynamicsTrajectory Function. However, I would like to ask you a question because the calculated torque does not seem to be the correct value.
<?xml version="1.0" encoding="utf-8"?>
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner ([email protected])
Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578_kink
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
<robot
name="ur10e">
<link name="base_link">
<visual>
<geometry>
<mesh filename="package://ur10e_description/meshes/ur10e/visual/base.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur10e_description/meshes/ur10e/collision/base.stl"/>
</geometry>
</collision>
<inertial>
<mass value="4.0"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="0.0061063308908" ixy="0.0" ixz="0.0" iyy="0.0061063308908" iyz="0.0" izz="0.01125"/>
</inertial>
</link>
<joint name="shoulder_pan_joint" type="revolute">
<parent link="base_link"/>
<child link="shoulder_link"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.181"/>
<axis xyz="0 0 1"/>
<!-- <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/> -->
<limit effort="10000.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="shoulder_link">
<visual>
<geometry>
<mesh filename="package://ur10e_description/meshes/ur10e/visual/shoulder.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur10e_description/meshes/ur10e/collision/shoulder.stl"/>
</geometry>
</collision>
<inertial>
<mass value="7.778"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<inertia ixx="0.03147431257693659" ixy="0.0" ixz="0.0" iyy="0.03147431257693659" iyz="0.0" izz="0.021875624999999996"/>
</inertial>
</link>
<joint name="shoulder_lift_joint" type="revolute">
<parent link="shoulder_link"/>
<child link="upper_arm_link"/>
<origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0 0.176 0.0"/>
<axis xyz="0 1 0"/>
<!-- <limit effort="330.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/> -->
<limit effort="10000.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="upper_arm_link">
<visual>
<geometry>
<mesh filename="package://ur10e_description/meshes/ur10e/visual/upperarm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur10e_description/meshes/ur10e/collision/upperarm.stl"/>
</geometry>
</collision>
<inertial>
<mass value="12.93"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.3065"/>
<inertia ixx="0.4230737407704359" ixy="0.0" ixz="0.0" iyy="0.4230737407704359" iyz="0.0" izz="0.03636562499999999"/>
</inertial>
</link>
<joint name="elbow_joint" type="revolute">
<parent link="upper_arm_link"/>
<child link="forearm_link"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.137 0.613"/>
<axis xyz="0 1 0"/>
<!-- <limit effort="150.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.14"/> -->
<limit effort="10000.0" lower="-3.141592653589793" upper="3.141592653589793" velocity="3.14"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="forearm_link">
<visual>
<geometry>
<mesh filename="package://ur10e_description/meshes/ur10e/visual/forearm.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur10e_description/meshes/ur10e/collision/forearm.stl"/>
</geometry>
</collision>
<inertial>
<mass value="3.87"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.2855"/>
<inertia ixx="0.11059036576383598" ixy="0.0" ixz="0.0" iyy="0.11059036576383598" iyz="0.0" izz="0.010884375"/>
</inertial>
</link>
<joint name="wrist_1_joint" type="revolute">
<parent link="forearm_link"/>
<child link="wrist_1_link"/>
<origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0 0.0 0.571"/>
<axis xyz="0 1 0"/>
<!-- <limit effort="54.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.28"/> -->
<limit effort="10000.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.28"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="wrist_1_link">
<visual>
<geometry>
<mesh filename="package://ur10e_description/meshes/ur10e/visual/wrist1.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur10e_description/meshes/ur10e/collision/wrist1.stl"/>
</geometry>
</collision>
<inertial>
<mass value="1.96"/>
<origin rpy="0 0 0" xyz="0.0 0.135 0.0"/>
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
</inertial>
</link>
<joint name="wrist_2_joint" type="revolute">
<parent link="wrist_1_link"/>
<child link="wrist_2_link"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.135 0.0"/>
<axis xyz="0 0 -1"/>
<!-- <limit effort="54.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.28"/> -->
<limit effort="10000.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.28"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="wrist_2_link">
<visual>
<geometry>
<mesh filename="package://ur10e_description/meshes/ur10e/visual/wrist2.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur10e_description/meshes/ur10e/collision/wrist2.stl"/>
</geometry>
</collision>
<inertial>
<mass value="1.96"/>
<origin rpy="0 0 0" xyz="0.0 0.0 0.12"/>
<inertia ixx="0.005108247956699999" ixy="0.0" ixz="0.0" iyy="0.005108247956699999" iyz="0.0" izz="0.005512499999999999"/>
</inertial>
</link>
<joint name="wrist_3_joint" type="revolute">
<parent link="wrist_2_link"/>
<child link="wrist_3_link"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.12"/>
<axis xyz="0 1 0"/>
<!-- <limit effort="54.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.28"/> -->
<limit effort="10000.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="6.28"/>
<dynamics damping="0.0" friction="0.0"/>
</joint>
<link name="wrist_3_link">
<visual>
<geometry>
<mesh filename="package://ur10e_description/meshes/ur10e/visual/wrist3.dae"/>
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur10e_description/meshes/ur10e/collision/wrist3.stl"/>
</geometry>
</collision>
<inertial>
<mass value="0.202"/>
<origin rpy="1.5707963267948966 0 0" xyz="0.0 0.092 0.0"/>
<inertia ixx="0.00014434577559500002" ixy="0.0" ixz="0.0" iyy="0.00014434577559500002" iyz="0.0" izz="0.00020452500000000002"/>
</inertial>
</link>
<joint name="ee_fixed_joint" type="fixed">
<parent link="wrist_3_link"/>
<child link="ee_link"/>
<origin rpy="0.0 0.0 1.5707963267948966" xyz="0.0 0.117 0.0"/>
</joint>
<link name="ee_link">
<collision>
<geometry>
<box size="0.01 0.01 0.01"/>
</geometry>
<origin rpy="0 0 0" xyz="-0.01 0 0"/>
</collision>
</link>
</robot>
%init pose to end pose torque compute
thetastart = [0; 0; 0; 0; 0; 0];
thetaend = [pi/2;0; 0;0;0;0];
% Total time of the motion in seconds from rest to rest
Tf = 5;
%The number of points N > 1 (Start and stop) in the discrete representation of the trajectory
dt = 0.01;
%Create a trajectory to follow
N = Tf / dt;
%he time-scaling method, where 3 indicates cubic third-order polynomial) time scaling and 5 indicates quintic (fifth-order polynomial) time scaling.
method = 5;
traj = JointTrajectory(thetastart, thetaend, Tf, N, method);
thetamat = traj;
dthetamat = zeros(1000, 6);
ddthetamat = zeros(1000, 6);
dt = Tf / (N - 1);
for i = 1: N - 1
dthetamat(i + 1, :) = (thetamat(i + 1, :) - thetamat(i, :)) / dt;
ddthetamat(i + 1, :) = (dthetamat(i + 1, :) - dthetamat(i, :)) / dt;
end
g = [0; 0; -9.8];
Ftipmat = zeros(N, 6);
taumat = InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, ...
g, Ftipmat, Mlist, Glist, Slist)
%Output using matplotlib to plot the joint forces/torques
time=0: dt: Tf;
plot(time, taumat(:, 1), 'b')
hold on
plot(time, taumat(:, 2), 'g')
plot(time, taumat(:, 3), 'r')
plot(time, taumat(:, 4), 'y')
plot(time, taumat(:, 5), 'k')
plot(time, taumat(:, 6), 'm')
title('Plot for Torque Trajectories')
xlabel('Time')
ylabel('Torque')
legend('Tau1', 'Tau2', 'Tau3','Tau4','Tau5','Tau6')
0 -71.0770 -23.0659 0 -0.0000 0
0.0099 -71.0780 -23.0660 -0.0000 0.0000 0
0.0592 -71.0828 -23.0665 -0.0003 0.0001 0
0.1177 -71.0886 -23.0670 -0.0005 0.0002 0
0.1755 -71.0943 -23.0676 -0.0008 0.0003 0
0.2327 -71.0999 -23.0682 -0.0010 0.0004 0
0.2891 -71.1054 -23.0687 -0.0013 0.0005 0
0.3448 -71.1109 -23.0693 -0.0015 0.0005 0
0.3998 -71.1163 -23.0698 -0.0017 0.0006 0
0.4541 -71.1216 -23.0704 -0.0020 0.0007 0
0.5077 -71.1269 -23.0709 -0.0022 0.0008 0
0.5607 -71.1321 -23.0714 -0.0025 0.0009 0
0.6129 -71.1373 -23.0719 -0.0027 0.0010 0
0.6644 -71.1423 -23.0724 -0.0029 0.0010 0
0.7153 -71.1473 -23.0730 -0.0031 0.0011 0
0.7655 -71.1523 -23.0735 -0.0034 0.0012 0
0.8150 -71.1572 -23.0740 -0.0036 0.0013 0
0.8639 -71.1620 -23.0745 -0.0038 0.0013 0
0.9120 -71.1667 -23.0750 -0.0040 0.0014 0
0.9596 -71.1714 -23.0755 -0.0042 0.0015 0
1.0064 -71.1761 -23.0759 -0.0044 0.0016 0
1.0526 -71.1806 -23.0764 -0.0047 0.0016 0
1.0981 -71.1851 -23.0769 -0.0049 0.0017 0
1.1429 -71.1896 -23.0774 -0.0051 0.0018 0
1.1872 -71.1940 -23.0779 -0.0053 0.0019 0
1.2307 -71.1983 -23.0784 -0.0055 0.0019 0
1.2736 -71.2026 -23.0789 -0.0057 0.0020 0
1.3159 -71.2068 -23.0794 -0.0059 0.0021 0
1.3575 -71.2110 -23.0798 -0.0061 0.0021 0
1.3985 -71.2151 -23.0803 -0.0063 0.0022 0
1.4388 -71.2192 -23.0808 -0.0065 0.0023 0
1.4786 -71.2232 -23.0813 -0.0067 0.0023 0
1.5176 -71.2271 -23.0818 -0.0069 0.0024 0
1.5561 -71.2310 -23.0823 -0.0071 0.0025 0
1.5939 -71.2349 -23.0828 -0.0073 0.0025 0
1.6311 -71.2387 -23.0833 -0.0075 0.0026 0
1.6677 -71.2424 -23.0839 -0.0077 0.0026 0
1.7037 -71.2461 -23.0844 -0.0079 0.0027 0
1.7391 -71.2498 -23.0849 -0.0081 0.0028 0
1.7738 -71.2534 -23.0854 -0.0083 0.0028 0
1.8080 -71.2570 -23.0860 -0.0085 0.0029 0
1.8415 -71.2605 -23.0865 -0.0087 0.0029 0
1.8744 -71.2639 -23.0871 -0.0089 0.0030 0
1.9068 -71.2674 -23.0877 -0.0091 0.0030 0
1.9385 -71.2708 -23.0882 -0.0094 0.0031 0
1.9697 -71.2741 -23.0888 -0.0096 0.0032 0
2.0002 -71.2774 -23.0894 -0.0098 0.0032 0
2.0302 -71.2807 -23.0900 -0.0100 0.0033 0
2.0596 -71.2839 -23.0907 -0.0102 0.0033 0
2.0884 -71.2871 -23.0913 -0.0104 0.0034 0
2.1166 -71.2902 -23.0919 -0.0107 0.0034 0
2.1442 -71.2933 -23.0926 -0.0109 0.0035 0
2.1713 -71.2964 -23.0933 -0.0111 0.0035 0
2.1978 -71.2994 -23.0940 -0.0113 0.0036 0
2.2238 -71.3024 -23.0947 -0.0116 0.0036 0
2.2491 -71.3054 -23.0954 -0.0118 0.0037 0
2.2739 -71.3083 -23.0961 -0.0121 0.0037 0
2.2982 -71.3112 -23.0969 -0.0123 0.0038 0
2.3219 -71.3140 -23.0976 -0.0125 0.0038 0
2.3450 -71.3168 -23.0984 -0.0128 0.0039 0
2.3676 -71.3196 -23.0992 -0.0130 0.0039 0
2.3897 -71.3224 -23.1000 -0.0133 0.0040 0
2.4112 -71.3251 -23.1008 -0.0136 0.0040 0
2.4322 -71.3278 -23.1017 -0.0138 0.0041 0
2.4526 -71.3305 -23.1025 -0.0141 0.0041 0
2.4725 -71.3331 -23.1034 -0.0144 0.0042 0
2.4918 -71.3357 -23.1043 -0.0146 0.0042 0
2.5106 -71.3383 -23.1052 -0.0149 0.0043 0
2.5289 -71.3409 -23.1062 -0.0152 0.0043 0
2.5467 -71.3434 -23.1071 -0.0155 0.0044 0
2.5640 -71.3459 -23.1081 -0.0158 0.0044 0
2.5807 -71.3484 -23.1091 -0.0161 0.0044 0
2.5969 -71.3508 -23.1101 -0.0164 0.0045 0
2.6126 -71.3533 -23.1112 -0.0167 0.0045 0
2.6278 -71.3557 -23.1122 -0.0170 0.0046 0
2.6425 -71.3580 -23.1133 -0.0173 0.0046 0
2.6567 -71.3604 -23.1144 -0.0176 0.0047 0
2.6704 -71.3627 -23.1155 -0.0180 0.0047 0
2.6836 -71.3650 -23.1167 -0.0183 0.0048 0
2.6962 -71.3673 -23.1178 -0.0186 0.0048 0
2.7084 -71.3696 -23.1190 -0.0190 0.0049 0
2.7201 -71.3718 -23.1202 -0.0193 0.0049 0
2.7314 -71.3741 -23.1215 -0.0197 0.0049 0
2.7421 -71.3763 -23.1227 -0.0200 0.0050 0
2.7524 -71.3784 -23.1240 -0.0204 0.0050 0
2.7621 -71.3806 -23.1253 -0.0208 0.0051 0
2.7714 -71.3828 -23.1266 -0.0211 0.0051 0
2.7803 -71.3849 -23.1279 -0.0215 0.0052 0
2.7886 -71.3870 -23.1293 -0.0219 0.0052 0
2.7965 -71.3891 -23.1307 -0.0223 0.0052 0
2.8039 -71.3911 -23.1321 -0.0227 0.0053 0
2.8109 -71.3932 -23.1335 -0.0231 0.0053 0
2.8174 -71.3952 -23.1350 -0.0235 0.0054 0
2.8235 -71.3972 -23.1365 -0.0239 0.0054 0
2.8291 -71.3992 -23.1380 -0.0243 0.0055 0
2.8342 -71.4012 -23.1395 -0.0247 0.0055 0
2.8389 -71.4031 -23.1410 -0.0251 0.0056 0
2.8432 -71.4051 -23.1426 -0.0256 0.0056 0
2.8470 -71.4070 -23.1442 -0.0260 0.0056 0
2.8504 -71.4089 -23.1458 -0.0264 0.0057 0
2.8533 -71.4108 -23.1474 -0.0269 0.0057 0
2.8558 -71.4127 -23.1491 -0.0273 0.0058 0
2.8579 -71.4145 -23.1507 -0.0278 0.0058 0
2.8596 -71.4164 -23.1524 -0.0282 0.0058 0
2.8608 -71.4182 -23.1542 -0.0287 0.0059 0
2.8616 -71.4200 -23.1559 -0.0292 0.0059 0
2.8620 -71.4218 -23.1577 -0.0296 0.0060 0
2.8620 -71.4236 -23.1594 -0.0301 0.0060 0
2.8616 -71.4253 -23.1612 -0.0306 0.0061 0
2.8607 -71.4271 -23.1630 -0.0311 0.0061 0
2.8595 -71.4288 -23.1649 -0.0316 0.0061 0
2.8578 -71.4305 -23.1667 -0.0321 0.0062 0
2.8558 -71.4322 -23.1686 -0.0326 0.0062 0
2.8533 -71.4339 -23.1705 -0.0331 0.0063 0
2.8505 -71.4356 -23.1724 -0.0336 0.0063 0
2.8472 -71.4372 -23.1744 -0.0341 0.0064 0
2.8436 -71.4388 -23.1763 -0.0346 0.0064 0
2.8396 -71.4404 -23.1783 -0.0352 0.0064 0
2.8352 -71.4420 -23.1803 -0.0357 0.0065 0
2.8304 -71.4436 -23.1823 -0.0362 0.0065 0
2.8253 -71.4452 -23.1843 -0.0368 0.0066 0
2.8197 -71.4467 -23.1863 -0.0373 0.0066 0
2.8138 -71.4483 -23.1884 -0.0378 0.0066 0
2.8076 -71.4498 -23.1904 -0.0384 0.0067 0
2.8009 -71.4513 -23.1925 -0.0389 0.0067 0
2.7939 -71.4527 -23.1946 -0.0395 0.0068 0
2.7866 -71.4542 -23.1967 -0.0400 0.0068 0
2.7789 -71.4556 -23.1988 -0.0406 0.0068 0
2.7708 -71.4570 -23.2010 -0.0412 0.0069 0
2.7624 -71.4584 -23.2031 -0.0417 0.0069 0
2.7536 -71.4598 -23.2053 -0.0423 0.0070 0
2.7445 -71.4612 -23.2074 -0.0429 0.0070 0
2.7350 -71.4625 -23.2096 -0.0434 0.0070 0
2.7252 -71.4639 -23.2118 -0.0440 0.0071 0
2.7151 -71.4652 -23.2140 -0.0446 0.0071 0
2.7046 -71.4665 -23.2162 -0.0452 0.0072 0
2.6938 -71.4677 -23.2184 -0.0457 0.0072 0
2.6826 -71.4690 -23.2207 -0.0463 0.0072 0
2.6712 -71.4702 -23.2229 -0.0469 0.0073 0
2.6594 -71.4714 -23.2252 -0.0475 0.0073 0
2.6473 -71.4726 -23.2274 -0.0481 0.0073 0
2.6349 -71.4738 -23.2297 -0.0487 0.0074 0
2.6221 -71.4749 -23.2319 -0.0493 0.0074 0
2.6091 -71.4760 -23.2342 -0.0498 0.0075 0
2.5957 -71.4771 -23.2365 -0.0504 0.0075 0
2.5820 -71.4782 -23.2388 -0.0510 0.0075 0
2.5681 -71.4792 -23.2411 -0.0516 0.0076 0
2.5538 -71.4803 -23.2433 -0.0522 0.0076 0
2.5392 -71.4813 -23.2456 -0.0528 0.0076 0
2.5244 -71.4822 -23.2479 -0.0534 0.0077 0
2.5092 -71.4832 -23.2502 -0.0540 0.0077 0
2.4938 -71.4841 -23.2525 -0.0546 0.0077 0
2.4781 -71.4850 -23.2548 -0.0552 0.0078 0
2.4621 -71.4859 -23.2571 -0.0558 0.0078 0
2.4458 -71.4868 -23.2594 -0.0563 0.0078 0
2.4292 -71.4876 -23.2617 -0.0569 0.0079 0
2.4124 -71.4884 -23.2640 -0.0575 0.0079 0
2.3952 -71.4892 -23.2663 -0.0581 0.0079 0
2.3779 -71.4899 -23.2685 -0.0587 0.0080 0
2.3602 -71.4906 -23.2708 -0.0593 0.0080 0
2.3423 -71.4913 -23.2731 -0.0599 0.0080 0
2.3241 -71.4920 -23.2754 -0.0604 0.0081 0
2.3057 -71.4926 -23.2776 -0.0610 0.0081 0
2.2870 -71.4932 -23.2799 -0.0616 0.0081 0
2.2681 -71.4938 -23.2821 -0.0622 0.0081 0
2.2489 -71.4943 -23.2844 -0.0627 0.0082 0
2.2295 -71.4949 -23.2866 -0.0633 0.0082 0
2.2098 -71.4953 -23.2888 -0.0639 0.0082 0
2.1899 -71.4958 -23.2910 -0.0644 0.0082 0
2.1698 -71.4962 -23.2933 -0.0650 0.0083 0
2.1494 -71.4966 -23.2954 -0.0655 0.0083 0
2.1288 -71.4970 -23.2976 -0.0661 0.0083 0
2.1079 -71.4973 -23.2998 -0.0666 0.0083 0
2.0869 -71.4976 -23.3019 -0.0672 0.0084 0
2.0656 -71.4978 -23.3041 -0.0677 0.0084 0
2.0441 -71.4981 -23.3062 -0.0683 0.0084 0
2.0224 -71.4983 -23.3083 -0.0688 0.0084 0
2.0004 -71.4984 -23.3104 -0.0693 0.0085 0
1.9783 -71.4985 -23.3125 -0.0699 0.0085 0
1.9559 -71.4986 -23.3146 -0.0704 0.0085 0
1.9334 -71.4987 -23.3166 -0.0709 0.0085 0
1.9106 -71.4987 -23.3187 -0.0714 0.0085 0
1.8876 -71.4987 -23.3207 -0.0719 0.0086 0
1.8645 -71.4986 -23.3227 -0.0724 0.0086 0
1.8411 -71.4985 -23.3246 -0.0729 0.0086 0
1.8176 -71.4984 -23.3266 -0.0734 0.0086 0
1.7938 -71.4983 -23.3285 -0.0738 0.0086 0
1.7699 -71.4981 -23.3304 -0.0743 0.0086 0
1.7458 -71.4978 -23.3323 -0.0748 0.0086 0
1.7215 -71.4975 -23.3342 -0.0753 0.0087 0
1.6971 -71.4972 -23.3360 -0.0757 0.0087 0
1.6724 -71.4969 -23.3379 -0.0762 0.0087 0
1.6476 -71.4965 -23.3397 -0.0766 0.0087 0
1.6227 -71.4960 -23.3414 -0.0770 0.0087 0
1.5975 -71.4956 -23.3432 -0.0775 0.0087 0
1.5722 -71.4950 -23.3449 -0.0779 0.0087 0
1.5468 -71.4945 -23.3466 -0.0783 0.0087 0
1.5212 -71.4939 -23.3483 -0.0787 0.0087 0
1.4954 -71.4932 -23.3499 -0.0791 0.0087 0
1.4695 -71.4926 -23.3515 -0.0795 0.0087 0
1.4434 -71.4919 -23.3531 -0.0799 0.0087 0
1.4172 -71.4911 -23.3547 -0.0802 0.0087 0
1.3908 -71.4903 -23.3562 -0.0806 0.0087 0
1.3643 -71.4894 -23.3577 -0.0810 0.0087 0
1.3377 -71.4886 -23.3592 -0.0813 0.0087 0
1.3109 -71.4876 -23.3606 -0.0817 0.0087 0
1.2841 -71.4867 -23.3620 -0.0820 0.0087 0
1.2570 -71.4856 -23.3634 -0.0823 0.0087 0
1.2299 -71.4846 -23.3647 -0.0826 0.0087 0
1.2026 -71.4835 -23.3660 -0.0829 0.0087 0
1.1752 -71.4823 -23.3673 -0.0832 0.0087 0
1.1477 -71.4811 -23.3685 -0.0835 0.0087 0
1.1201 -71.4799 -23.3697 -0.0838 0.0087 0
1.0924 -71.4786 -23.3709 -0.0841 0.0087 0
1.0645 -71.4773 -23.3721 -0.0843 0.0087 0
1.0366 -71.4759 -23.3732 -0.0846 0.0087 0
1.0085 -71.4745 -23.3742 -0.0848 0.0087 0
0.9804 -71.4731 -23.3753 -0.0850 0.0086 0
0.9522 -71.4716 -23.3763 -0.0853 0.0086 0
0.9238 -71.4700 -23.3772 -0.0855 0.0086 0
0.8954 -71.4685 -23.3782 -0.0857 0.0086 0
0.8669 -71.4668 -23.3790 -0.0859 0.0086 0
0.8383 -71.4652 -23.3799 -0.0860 0.0086 0
0.8096 -71.4634 -23.3807 -0.0862 0.0085 0
0.7809 -71.4617 -23.3815 -0.0864 0.0085 0
0.7520 -71.4599 -23.3822 -0.0865 0.0085 0
0.7231 -71.4580 -23.3829 -0.0867 0.0085 0
0.6942 -71.4561 -23.3836 -0.0868 0.0085 0
0.6651 -71.4542 -23.3842 -0.0869 0.0084 0
0.6360 -71.4522 -23.3848 -0.0870 0.0084 0
0.6068 -71.4501 -23.3853 -0.0871 0.0084 0
0.5776 -71.4480 -23.3858 -0.0872 0.0084 0
0.5483 -71.4459 -23.3863 -0.0873 0.0083 0
0.5190 -71.4438 -23.3867 -0.0873 0.0083 0
0.4896 -71.4415 -23.3871 -0.0874 0.0083 0
0.4602 -71.4393 -23.3875 -0.0874 0.0082 0
0.4307 -71.4370 -23.3878 -0.0875 0.0082 0
0.4012 -71.4346 -23.3880 -0.0875 0.0082 0
0.3716 -71.4322 -23.3882 -0.0875 0.0081 0
0.3420 -71.4298 -23.3884 -0.0875 0.0081 0
0.3124 -71.4273 -23.3886 -0.0875 0.0081 0
0.2827 -71.4248 -23.3887 -0.0875 0.0080 0
0.2530 -71.4222 -23.3887 -0.0874 0.0080 0
0.2233 -71.4196 -23.3887 -0.0874 0.0080 0
0.1936 -71.4170 -23.3887 -0.0873 0.0079 0
0.1638 -71.4143 -23.3887 -0.0872 0.0079 0
0.1341 -71.4115 -23.3885 -0.0872 0.0078 0
0.1043 -71.4088 -23.3884 -0.0871 0.0078 0
0.0745 -71.4059 -23.3882 -0.0870 0.0077 0
0.0447 -71.4031 -23.3880 -0.0869 0.0077 0
0.0149 -71.4002 -23.3877 -0.0867 0.0077 0
-0.0149 -71.3972 -23.3874 -0.0866 0.0076 0
-0.0447 -71.3942 -23.3870 -0.0864 0.0076 0
-0.0745 -71.3912 -23.3866 -0.0863 0.0075 0
-0.1043 -71.3881 -23.3862 -0.0861 0.0075 0
-0.1341 -71.3850 -23.3857 -0.0859 0.0074 0
-0.1638 -71.3819 -23.3852 -0.0858 0.0074 0
-0.1936 -71.3787 -23.3846 -0.0855 0.0073 0
-0.2233 -71.3754 -23.3840 -0.0853 0.0073 0
-0.2530 -71.3722 -23.3834 -0.0851 0.0072 0
-0.2827 -71.3689 -23.3827 -0.0849 0.0071 0
-0.3124 -71.3655 -23.3820 -0.0846 0.0071 0
-0.3420 -71.3621 -23.3812 -0.0844 0.0070 0
-0.3716 -71.3587 -23.3804 -0.0841 0.0070 0
-0.4012 -71.3553 -23.3796 -0.0838 0.0069 0
-0.4307 -71.3518 -23.3787 -0.0835 0.0069 0
-0.4602 -71.3482 -23.3778 -0.0832 0.0068 0
-0.4896 -71.3447 -23.3768 -0.0829 0.0067 0
-0.5190 -71.3411 -23.3758 -0.0826 0.0067 0
-0.5483 -71.3374 -23.3748 -0.0823 0.0066 0
-0.5776 -71.3338 -23.3737 -0.0819 0.0065 0
-0.6068 -71.3301 -23.3726 -0.0816 0.0065 0
-0.6360 -71.3263 -23.3714 -0.0812 0.0064 0
-0.6651 -71.3226 -23.3702 -0.0809 0.0063 0
-0.6942 -71.3188 -23.3690 -0.0805 0.0063 0
-0.7231 -71.3149 -23.3677 -0.0801 0.0062 0
-0.7520 -71.3111 -23.3664 -0.0797 0.0061 0
-0.7809 -71.3072 -23.3651 -0.0793 0.0061 0
-0.8096 -71.3033 -23.3637 -0.0788 0.0060 0
-0.8383 -71.2993 -23.3623 -0.0784 0.0059 0
-0.8669 -71.2953 -23.3608 -0.0780 0.0059 0
-0.8954 -71.2913 -23.3593 -0.0775 0.0058 0
-0.9238 -71.2873 -23.3578 -0.0771 0.0057 0
-0.9522 -71.2832 -23.3563 -0.0766 0.0056 0
-0.9804 -71.2792 -23.3547 -0.0761 0.0056 0
-1.0085 -71.2750 -23.3531 -0.0756 0.0055 0
-1.0366 -71.2709 -23.3514 -0.0751 0.0054 0
-1.0645 -71.2667 -23.3497 -0.0746 0.0053 0
-1.0924 -71.2626 -23.3480 -0.0741 0.0052 0
-1.1201 -71.2584 -23.3462 -0.0736 0.0052 0
-1.1477 -71.2541 -23.3445 -0.0731 0.0051 0
-1.1752 -71.2499 -23.3426 -0.0725 0.0050 0
-1.2026 -71.2456 -23.3408 -0.0720 0.0049 0
-1.2299 -71.2413 -23.3389 -0.0714 0.0048 0
-1.2570 -71.2370 -23.3370 -0.0709 0.0048 0
-1.2841 -71.2327 -23.3351 -0.0703 0.0047 0
-1.3109 -71.2284 -23.3331 -0.0697 0.0046 0
-1.3377 -71.2240 -23.3311 -0.0692 0.0045 0
-1.3643 -71.2196 -23.3291 -0.0686 0.0044 0
-1.3908 -71.2152 -23.3271 -0.0680 0.0044 0
-1.4172 -71.2108 -23.3250 -0.0674 0.0043 0
-1.4434 -71.2064 -23.3229 -0.0668 0.0042 0
-1.4695 -71.2020 -23.3208 -0.0661 0.0041 0
-1.4954 -71.1976 -23.3186 -0.0655 0.0040 0
-1.5212 -71.1931 -23.3164 -0.0649 0.0039 0
-1.5468 -71.1886 -23.3142 -0.0643 0.0038 0
-1.5722 -71.1842 -23.3120 -0.0636 0.0038 0
-1.5975 -71.1797 -23.3098 -0.0630 0.0037 0
-1.6227 -71.1752 -23.3075 -0.0623 0.0036 0
-1.6476 -71.1707 -23.3052 -0.0616 0.0035 0
-1.6724 -71.1662 -23.3029 -0.0610 0.0034 0
-1.6971 -71.1617 -23.3006 -0.0603 0.0033 0
-1.7215 -71.1572 -23.2982 -0.0596 0.0032 0
-1.7458 -71.1527 -23.2959 -0.0590 0.0031 0
-1.7699 -71.1482 -23.2935 -0.0583 0.0031 0
-1.7938 -71.1436 -23.2911 -0.0576 0.0030 0
-1.8176 -71.1391 -23.2887 -0.0569 0.0029 0
-1.8411 -71.1346 -23.2862 -0.0562 0.0028 0
-1.8645 -71.1301 -23.2838 -0.0555 0.0027 0
-1.8876 -71.1256 -23.2813 -0.0548 0.0026 0
-1.9106 -71.1211 -23.2788 -0.0541 0.0025 0
-1.9334 -71.1166 -23.2763 -0.0534 0.0024 0
-1.9559 -71.1121 -23.2738 -0.0527 0.0023 0
-1.9783 -71.1076 -23.2713 -0.0519 0.0023 0
-2.0004 -71.1031 -23.2688 -0.0512 0.0022 0
-2.0224 -71.0986 -23.2662 -0.0505 0.0021 0
-2.0441 -71.0941 -23.2637 -0.0498 0.0020 0
-2.0656 -71.0897 -23.2611 -0.0490 0.0019 0
-2.0869 -71.0852 -23.2586 -0.0483 0.0018 0
-2.1079 -71.0808 -23.2560 -0.0476 0.0017 0
-2.1288 -71.0763 -23.2534 -0.0469 0.0016 0
-2.1494 -71.0719 -23.2508 -0.0461 0.0015 0
-2.1698 -71.0675 -23.2482 -0.0454 0.0014 0
-2.1899 -71.0631 -23.2456 -0.0446 0.0014 0
-2.2098 -71.0587 -23.2430 -0.0439 0.0013 0
-2.2295 -71.0544 -23.2403 -0.0432 0.0012 0
-2.2489 -71.0501 -23.2377 -0.0424 0.0011 0
-2.2681 -71.0457 -23.2351 -0.0417 0.0010 0
-2.2870 -71.0414 -23.2325 -0.0409 0.0009 0
-2.3057 -71.0372 -23.2299 -0.0402 0.0008 0
-2.3241 -71.0329 -23.2272 -0.0395 0.0007 0
-2.3423 -71.0287 -23.2246 -0.0387 0.0007 0
-2.3602 -71.0244 -23.2220 -0.0380 0.0006 0
-2.3779 -71.0203 -23.2193 -0.0373 0.0005 0
-2.3952 -71.0161 -23.2167 -0.0365 0.0004 0
-2.4124 -71.0120 -23.2141 -0.0358 0.0003 0
-2.4292 -71.0079 -23.2115 -0.0350 0.0002 0
-2.4458 -71.0038 -23.2089 -0.0343 0.0001 0
-2.4621 -70.9997 -23.2062 -0.0336 0.0001 0
-2.4781 -70.9957 -23.2036 -0.0328 -0.0000 0
-2.4938 -70.9917 -23.2010 -0.0321 -0.0001 0
-2.5092 -70.9878 -23.1984 -0.0314 -0.0002 0
-2.5244 -70.9838 -23.1959 -0.0307 -0.0003 0
-2.5392 -70.9799 -23.1933 -0.0300 -0.0003 0
-2.5538 -70.9761 -23.1907 -0.0292 -0.0004 0
-2.5681 -70.9722 -23.1881 -0.0285 -0.0005 0
-2.5820 -70.9685 -23.1856 -0.0278 -0.0006 0
-2.5957 -70.9647 -23.1830 -0.0271 -0.0007 0
-2.6091 -70.9610 -23.1805 -0.0264 -0.0007 0
-2.6221 -70.9573 -23.1780 -0.0257 -0.0008 0
-2.6349 -70.9537 -23.1755 -0.0250 -0.0009 0
-2.6473 -70.9501 -23.1730 -0.0243 -0.0010 0
-2.6594 -70.9465 -23.1705 -0.0236 -0.0010 0
-2.6712 -70.9430 -23.1680 -0.0229 -0.0011 0
-2.6826 -70.9396 -23.1656 -0.0222 -0.0012 0
-2.6938 -70.9361 -23.1631 -0.0216 -0.0013 0
-2.7046 -70.9328 -23.1607 -0.0209 -0.0013 0
-2.7151 -70.9294 -23.1583 -0.0202 -0.0014 0
-2.7252 -70.9262 -23.1559 -0.0196 -0.0015 0
-2.7350 -70.9229 -23.1536 -0.0189 -0.0015 0
-2.7445 -70.9197 -23.1512 -0.0183 -0.0016 0
-2.7536 -70.9166 -23.1489 -0.0176 -0.0017 0
-2.7624 -70.9135 -23.1466 -0.0170 -0.0018 0
-2.7708 -70.9104 -23.1443 -0.0163 -0.0018 0
-2.7789 -70.9075 -23.1420 -0.0157 -0.0019 0
-2.7866 -70.9045 -23.1397 -0.0151 -0.0019 0
-2.7939 -70.9016 -23.1375 -0.0145 -0.0020 0
-2.8009 -70.8988 -23.1353 -0.0139 -0.0021 0
-2.8076 -70.8960 -23.1331 -0.0133 -0.0021 0
-2.8138 -70.8933 -23.1309 -0.0127 -0.0022 0
-2.8197 -70.8906 -23.1288 -0.0121 -0.0022 0
-2.8253 -70.8880 -23.1267 -0.0115 -0.0023 0
-2.8304 -70.8855 -23.1246 -0.0109 -0.0024 0
-2.8352 -70.8830 -23.1225 -0.0104 -0.0024 0
-2.8396 -70.8805 -23.1204 -0.0098 -0.0025 0
-2.8436 -70.8782 -23.1184 -0.0093 -0.0025 0
-2.8472 -70.8758 -23.1164 -0.0087 -0.0026 0
-2.8505 -70.8736 -23.1144 -0.0082 -0.0026 0
-2.8533 -70.8714 -23.1125 -0.0076 -0.0027 0
-2.8558 -70.8692 -23.1106 -0.0071 -0.0027 0
-2.8578 -70.8672 -23.1087 -0.0066 -0.0028 0
-2.8595 -70.8652 -23.1068 -0.0061 -0.0028 0
-2.8607 -70.8632 -23.1050 -0.0056 -0.0029 0
-2.8616 -70.8613 -23.1032 -0.0051 -0.0029 0
-2.8620 -70.8595 -23.1014 -0.0046 -0.0030 0
-2.8620 -70.8577 -23.0996 -0.0042 -0.0030 0
-2.8616 -70.8561 -23.0979 -0.0037 -0.0030 0
-2.8608 -70.8544 -23.0962 -0.0032 -0.0031 0
-2.8596 -70.8529 -23.0945 -0.0028 -0.0031 0
-2.8579 -70.8514 -23.0929 -0.0024 -0.0032 0
-2.8558 -70.8500 -23.0913 -0.0019 -0.0032 0
-2.8533 -70.8486 -23.0897 -0.0015 -0.0032 0
-2.8504 -70.8473 -23.0882 -0.0011 -0.0033 0
-2.8470 -70.8461 -23.0867 -0.0007 -0.0033 0
-2.8432 -70.8450 -23.0852 -0.0003 -0.0033 0
-2.8389 -70.8439 -23.0837 0.0001 -0.0033 0
-2.8342 -70.8429 -23.0823 0.0005 -0.0034 0
-2.8291 -70.8419 -23.0809 0.0008 -0.0034 0
-2.8235 -70.8411 -23.0795 0.0012 -0.0034 0
-2.8174 -70.8403 -23.0782 0.0015 -0.0034 0
-2.8109 -70.8395 -23.0769 0.0019 -0.0035 0
-2.8039 -70.8389 -23.0756 0.0022 -0.0035 0
-2.7965 -70.8383 -23.0744 0.0025 -0.0035 0
-2.7886 -70.8378 -23.0732 0.0028 -0.0035 0
-2.7803 -70.8374 -23.0720 0.0031 -0.0035 0
-2.7714 -70.8370 -23.0709 0.0034 -0.0036 0
-2.7621 -70.8367 -23.0698 0.0037 -0.0036 0
-2.7524 -70.8365 -23.0687 0.0040 -0.0036 0
-2.7421 -70.8364 -23.0676 0.0042 -0.0036 0
-2.7314 -70.8363 -23.0666 0.0045 -0.0036 0
-2.7201 -70.8363 -23.0656 0.0047 -0.0036 0
-2.7084 -70.8364 -23.0647 0.0050 -0.0036 0
-2.6962 -70.8366 -23.0638 0.0052 -0.0036 0
-2.6836 -70.8368 -23.0629 0.0054 -0.0036 0
-2.6704 -70.8371 -23.0620 0.0056 -0.0036 0
-2.6567 -70.8375 -23.0612 0.0058 -0.0036 0
-2.6425 -70.8379 -23.0604 0.0060 -0.0036 0
-2.6278 -70.8385 -23.0596 0.0062 -0.0036 0
-2.6126 -70.8391 -23.0589 0.0064 -0.0036 0
-2.5969 -70.8398 -23.0582 0.0065 -0.0036 0
-2.5807 -70.8405 -23.0575 0.0067 -0.0036 0
-2.5640 -70.8414 -23.0569 0.0068 -0.0036 0
-2.5467 -70.8423 -23.0563 0.0070 -0.0036 0
-2.5289 -70.8433 -23.0557 0.0071 -0.0036 0
-2.5106 -70.8443 -23.0551 0.0072 -0.0036 0
-2.4918 -70.8455 -23.0546 0.0073 -0.0036 0
-2.4725 -70.8467 -23.0541 0.0074 -0.0036 0
-2.4526 -70.8480 -23.0536 0.0075 -0.0035 0
-2.4322 -70.8493 -23.0532 0.0076 -0.0035 0
-2.4112 -70.8508 -23.0528 0.0077 -0.0035 0
-2.3897 -70.8523 -23.0524 0.0077 -0.0035 0
-2.3676 -70.8539 -23.0521 0.0078 -0.0035 0
-2.3450 -70.8556 -23.0517 0.0079 -0.0034 0
-2.3219 -70.8573 -23.0514 0.0079 -0.0034 0
-2.2982 -70.8591 -23.0512 0.0079 -0.0034 0
-2.2739 -70.8610 -23.0509 0.0079 -0.0034 0
-2.2491 -70.8630 -23.0507 0.0080 -0.0033 0
-2.2238 -70.8650 -23.0505 0.0080 -0.0033 0
-2.1978 -70.8672 -23.0503 0.0080 -0.0033 0
-2.1713 -70.8694 -23.0502 0.0080 -0.0033 0
-2.1442 -70.8716 -23.0501 0.0080 -0.0032 0
-2.1166 -70.8740 -23.0500 0.0079 -0.0032 0
-2.0884 -70.8764 -23.0499 0.0079 -0.0032 0
-2.0596 -70.8789 -23.0498 0.0079 -0.0031 0
-2.0302 -70.8815 -23.0498 0.0078 -0.0031 0
-2.0002 -70.8841 -23.0498 0.0078 -0.0030 0
-1.9697 -70.8868 -23.0498 0.0077 -0.0030 0
-1.9385 -70.8896 -23.0499 0.0077 -0.0030 0
-1.9068 -70.8925 -23.0499 0.0076 -0.0029 0
-1.8744 -70.8954 -23.0500 0.0075 -0.0029 0
-1.8415 -70.8985 -23.0501 0.0074 -0.0028 0
-1.8080 -70.9015 -23.0502 0.0073 -0.0028 0
-1.7738 -70.9047 -23.0504 0.0072 -0.0027 0
-1.7391 -70.9079 -23.0505 0.0071 -0.0027 0
-1.7037 -70.9112 -23.0507 0.0070 -0.0026 0
-1.6677 -70.9146 -23.0509 0.0069 -0.0026 0
-1.6311 -70.9181 -23.0511 0.0068 -0.0025 0
-1.5939 -70.9216 -23.0513 0.0067 -0.0025 0
-1.5561 -70.9252 -23.0516 0.0065 -0.0024 0
-1.5176 -70.9288 -23.0519 0.0064 -0.0023 0
-1.4786 -70.9326 -23.0521 0.0062 -0.0023 0
-1.4388 -70.9364 -23.0524 0.0061 -0.0022 0
-1.3985 -70.9403 -23.0527 0.0059 -0.0022 0
-1.3575 -70.9442 -23.0531 0.0058 -0.0021 0
-1.3159 -70.9482 -23.0534 0.0056 -0.0020 0
-1.2736 -70.9523 -23.0538 0.0055 -0.0020 0
-1.2307 -70.9565 -23.0541 0.0053 -0.0019 0
-1.1872 -70.9607 -23.0545 0.0051 -0.0018 0
-1.1429 -70.9650 -23.0549 0.0049 -0.0018 0
-1.0981 -70.9694 -23.0553 0.0047 -0.0017 0
-1.0526 -70.9738 -23.0557 0.0046 -0.0016 0
-1.0064 -70.9783 -23.0561 0.0044 -0.0016 0
-0.9596 -70.9829 -23.0566 0.0042 -0.0015 0
-0.9120 -70.9875 -23.0570 0.0040 -0.0014 0
-0.8639 -70.9922 -23.0575 0.0038 -0.0013 0
-0.8150 -70.9970 -23.0579 0.0035 -0.0013 0
-0.7655 -71.0019 -23.0584 0.0033 -0.0012 0
-0.7153 -71.0068 -23.0589 0.0031 -0.0011 0
-0.6644 -71.0118 -23.0594 0.0029 -0.0010 0
-0.6129 -71.0168 -23.0599 0.0027 -0.0010 0
-0.5607 -71.0220 -23.0604 0.0024 -0.0009 0
-0.5077 -71.0272 -23.0609 0.0022 -0.0008 0
-0.4541 -71.0324 -23.0614 0.0020 -0.0007 0
-0.3998 -71.0378 -23.0620 0.0017 -0.0006 0
-0.3448 -71.0432 -23.0625 0.0015 -0.0005 0
-0.2891 -71.0486 -23.0630 0.0013 -0.0005 0
-0.2327 -71.0542 -23.0636 0.0010 -0.0004 0
-0.1755 -71.0598 -23.0642 0.0008 -0.0003 0
-0.1177 -71.0655 -23.0647 0.0005 -0.0002 0
-0.0592 -71.0712 -23.0653 0.0003 -0.0001 0
Since only the first joint rotates by pi/2, I think the torque absolute value of the first joint should increase and then decrease.
However, the value of the calculated torque is really confusing because it has a value that goes between positive and negative values. can you please help me?