完整代码为：
model Model7
import Modelica.Mechanics.MultiBody.;
import Modelica.Mechanics.MultiBody.Frames;
import Modelica.Blocks.Interfaces.;

// 全局设置，关闭重力
inner World world(
gravityType = Modelica.Mechanics.MultiBody.Types.GravityTypes.NoGravity
);

// 框架尺寸参数
parameter Real frameSize[3] = {3.0, 3.0, 1.6};

// 框架的八个顶点
Parts.FixedTranslation framePoints[8](
each animation = false,
r = {
{0, 0, 0}, // 左下前角
{frameSize[1], 0, 0}, // 右下前角
{frameSize[1], frameSize[2], 0}, // 右上后角
{0, frameSize[2], 0}, // 左上后角
{0, 0, frameSize[3]}, // 左上前角
{frameSize[1], 0, frameSize[3]}, // 右上前角
{frameSize[1], frameSize[2], frameSize[3]}, // 右上后角
{0, frameSize[2], frameSize[3]} // 左上后角
}
);

// 末端执行器尺寸参数
parameter Real endEffectorSize[3] = {0.3, 0.3, 0.2};

// 轨迹参数
parameter Real initialPose[6] = {1.7, 2.0, 1.2, 0.0, 0.0, 0.0};
parameter Real targetPose[6] = {1.0, 2.0, 1.4, 0.0, 0.0, 0.0};
parameter Real totalTime = 2;
parameter Real timeStep = 0.02;

// 五次多项式系数
Real coeffsX[6];
Real coeffsY[6];
Real coeffsZ[6];
Real coeffsRoll[6];
Real coeffsPitch[6];
Real coeffsYaw[6];

// 当前位姿
Real currentPose[6];

// 运动控制组件
Parts.Body endEffector(m=0.1, r_CM={0,0,0}) "末端执行器";
Modelica.Mechanics.MultiBody.Joints.FreeMotion freeMotion(
useQuaternions=false,
animation=false
);

// 传感器
Sensors.Distance ropeLengths[8];
Modelica.Blocks.Interfaces.RealOutput ropeLengthOutput[8] = ropeLengths.distance;
Modelica.Blocks.Interfaces.RealOutput poseOutput[6];

equation
// 轨迹系数计算
coeffsX = fifthOrderPolynomialCoefficients(0, initialPose[1], 0, 0, totalTime, targetPose[1], 0, 0);
coeffsY = fifthOrderPolynomialCoefficients(0, initialPose[2], 0, 0, totalTime, targetPose[2], 0, 0);
coeffsZ = fifthOrderPolynomialCoefficients(0, initialPose[3], 0, 0, totalTime, targetPose[3], 0, 0);
coeffsRoll = fifthOrderPolynomialCoefficients(0, initialPose[4], 0, 0, totalTime, targetPose[4], 0, 0);
coeffsPitch = fifthOrderPolynomialCoefficients(0, initialPose[5], 0, 0, totalTime, targetPose[5], 0, 0);
coeffsYaw = fifthOrderPolynomialCoefficients(0, initialPose[6], 0, 0, totalTime, targetPose[6], 0, 0);

// 实时位姿计算
currentPose[1] = fifthOrderPolynomialEvaluation(coeffsX, time);
currentPose[2] = fifthOrderPolynomialEvaluation(coeffsY, time);
currentPose[3] = fifthOrderPolynomialEvaluation(coeffsZ, time);
currentPose[4] = fifthOrderPolynomialEvaluation(coeffsRoll, time);
currentPose[5] = fifthOrderPolynomialEvaluation(coeffsPitch, time);
currentPose[6] = fifthOrderPolynomialEvaluation(coeffsYaw, time);

// 运动约束方程
freeMotion.r_rela = {currentPose[1], currentPose[2], currentPose[3]};
freeMotion.angles = {currentPose[4], currentPose[5], currentPose[6]};

// 连接关系
connect(world.frame_b, freeMotion.frame_a);
connect(freeMotion.frame_b, endEffector.frame_a);

// 框架连接
for i in 1:8 loop
connect(world.frame_b, framePoints[i].frame_a);
end for;

// 末端连接点
Parts.FixedTranslation endEffectorPoints[8](
each animation=true,
r = {
{-endEffectorSize[1]/2, -endEffectorSize[2]/2, -endEffectorSize[3]/2},
{ endEffectorSize[1]/2, -endEffectorSize[2]/2, -endEffectorSize[3]/2},
{ endEffectorSize[1]/2, endEffectorSize[2]/2, -endEffectorSize[3]/2},
{-endEffectorSize[1]/2, endEffectorSize[2]/2, -endEffectorSize[3]/2},
{-endEffectorSize[1]/2, -endEffectorSize[2]/2, endEffectorSize[3]/2},
{ endEffectorSize[1]/2, -endEffectorSize[2]/2, endEffectorSize[3]/2},
{ endEffectorSize[1]/2, endEffectorSize[2]/2, endEffectorSize[3]/2},
{-endEffectorSize[1]/2, endEffectorSize[2]/2, endEffectorSize[3]/2}
);

for i in 1:8 loop
connect(endEffector.frame_a, endEffectorPoints[i].frame_a);
connect(framePoints[i].frame_b, ropeLengths[i].frame_a);
connect(endEffectorPoints[i].frame_b, ropeLengths[i].frame_b);
end for;

// 位姿输出
poseOutput = currentPose;

annotation(
experiment(
StartTime = 0,
StopTime = totalTime,
Tolerance = 1e-6,
Interval = timeStep
)
);

protected
function fifthOrderPolynomialCoefficients
input Real t0; // 初始时间
input Real q0; // 初始位置
input Real v0; // 初始速度
input Real a0; // 初始加速度
input Real tf; // 终止时间
input Real qf; // 终止位置
input Real vf; // 终止速度
input Real af; // 终止加速度
output Real coeffs[6];

protected
Real T = tf - t0;
Real T2 = TT;
Real T3 = T2T;
Real T4 = T3T;
Real T5 = T4T;

algorithm
coeffs[1] := q0;
coeffs[2] := v0;
coeffs[3] := a0/2;
coeffs[4] := (20*(qf - q0) - (8vf + 12v0)T - (3af - a0)T2)/(2T3);
coeffs[5] := (30*(q0 - qf) + (14vf + 16v0)T + (3af - 2a0)T2)/(2T4);
coeffs[6] := (12(qf - q0) - 6*(vf + v0)*T - (af - a0)T2)/(2T5);
end fifthOrderPolynomialCoefficients;

function fifthOrderPolynomialEvaluation
input Real coeffs[6];
input Real t;
output Real value;
algorithm
value := coeffs[1] + coeffs[2]*t + coeffs[3]*t^2 + coeffs[4]*t^3 + coeffs[5]*t^4 + coeffs[6]*t^5;
end fifthOrderPolynomialEvaluation;
end Model7;