public static TrapezoidalJointTrajectoryGeneraterExec InitializeVelExec(uint joint_count, JointTrajectoryLimits limits, JointTrajectoryVelocityRequest request)
{
double[] a_max = (double[])limits.a_max.Clone();
double[] v_max = (double[])limits.v_max.Clone();
if (request.speed_ratio != 0.0)
{
for (int i = 0; i < joint_count; i++)
{
a_max[i] = a_max[i] * request.speed_ratio;
v_max[i] = v_max[i] * request.speed_ratio;
double req_vel = request.desired_velocity[i] * request.speed_ratio;
if (req_vel < v_max[i])
{
v_max[i] = req_vel;
}
if (req_vel > v_max[i])
{
throw new ArgumentException("req_vel must be less than or equal to v_max ");
}
}
}
else
{
for (int i = 0; i < joint_count; i++)
{
if (request.desired_velocity[i] > v_max[i])
{
throw new ArgumentException("req_vel must be less than or equal to v_max ");
}
if (request.desired_velocity[i] < v_max[i])
{
v_max[i] = request.desired_velocity[i];
}
}
}
double[] t1 = new double[joint_count];
double[] t2 = new double[joint_count];
double[] t3 = new double[joint_count];
double[] v1 = new double[joint_count];
double[] a1 = new double[joint_count];
double[] a3 = new double[joint_count];
for (int i = 0; i < joint_count; i++)
{
if (request.desired_velocity[i] == 0 && request.current_velocity[i] == 0)
{
continue;
}
solve_case5(request.current_velocity[i], request.desired_velocity[i], 0, request.timeout,
a_max[i], out v1[i], out a1[i], out a3[i], out t1[i], out t2[i], out t3[i]);
}
double t1_4 = t1.Max();
double t2_4 = t2.Max();
double t3_4 = t3.Max();
double[] a1_4 = new double[joint_count];
double[] a3_4 = new double[joint_count];
for (int i = 0; i < joint_count; i++)
{
solve_case6(request.current_velocity[i], v1[i], 0,
t1_4, t2_4, t3_4, out a1_4[i], out a3_4[i]);
}
var ret = new TrapezoidalJointTrajectoryGeneraterExec()
{
joint_count = joint_count,
t1 = t1_4,
t2 = t2_4,
t3 = t3_4,
x1 = request.current_position,
v1 = request.current_velocity,
v2 = v1,
v3 = new double[joint_count],
a1 = a1_4,
a3 = a3_4,
//xf = request.desired_position
};
return(ret);
}
public override bool UpdateDesiredVelocity(ref JointTrajectoryVelocityRequest request)
{
exec = TrapezoidalJointTrajectoryGeneraterCalc.InitializeVelExec(_joint_count, _limits, request);
return(true);
}