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