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); }