public override bool UpdateDesiredPosition(ref JointTrajectoryPositionRequest request)
{
if (request.desired_time != null)
{
throw new InvalidOperationException("desired_time not supported");
}
exec = TrapezoidalJointTrajectoryGeneraterCalc.InitializePosExec(_joint_count, _limits, request);
return(true);
}
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 static TrapezoidalJointTrajectoryGeneraterExec InitializePosExec(uint joint_count, JointTrajectoryLimits limits, JointTrajectoryPositionRequest request)
{
Debug.Assert(request.desired_time == null);
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;
if (request.max_velocity != null)
{
double req_vel = request.max_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.max_velocity[i] > v_max[i])
{
throw new ArgumentException("req_vel must be less than or equal to v_max ");
}
if (request.max_velocity[i] < v_max[i])
{
v_max[i] = request.max_velocity[i];
}
}
}
double[] dx = new double[joint_count];
for (int i = 0; i < joint_count; i++)
{
dx[i] = request.desired_position[i] - request.current_position[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 (dx[i] == 0 && request.desired_velocity[i] == 0 && request.current_velocity[i] == 0)
{
continue;
}
bool case2_success = solve_case2(request.current_position[i], request.desired_position[i], request.current_velocity[i], request.desired_velocity[i],
v_max[i], a_max[i], out v1[i], out a1[i], out a3[i], out t1[i], out t2[i], out t3[i]);
if (!case2_success)
{
bool case3_success = solve_case3(request.current_position[i], request.desired_position[i], request.current_velocity[i], request.desired_velocity[i],
a_max[i], out a1[i], out a3[i], out t1[i], out t3[i]);
t2[i] = 0;
v1[i] = 0;
if (!case3_success)
{
throw new ArgumentException("Invalid trajectory request");
}
}
}
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];
double[] v1_4 = new double[joint_count];
for (int i = 0; i < joint_count; i++)
{
solve_case4(request.current_position[i], request.desired_position[i], request.current_velocity[i], request.desired_velocity[i],
t1_4, t2_4, t3_4, out v1_4[i], 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_4,
v3 = request.desired_velocity,
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);
}
