public GetCartesianPathResponse(RobotState start_state, RobotTrajectory solution, double fraction, MoveItErrorCodes error_code)
{
this.start_state = start_state;
this.solution = solution;
this.fraction = fraction;
this.error_code = error_code;
}
public MotionPlanResponse(RobotState trajectory_start, string group_name, RobotTrajectory trajectory, double planning_time, MoveItErrorCodes error_code)
{
this.trajectory_start = trajectory_start;
this.group_name = group_name;
this.trajectory = trajectory;
this.planning_time = planning_time;
this.error_code = error_code;
}
/// <summary>
/// Execute trajectories from RobotMoveActionGoal topic.
///
/// Execution will iterate through every robot pose in the trajectory pose
/// array while updating the joint values on the simulated robot.
///
/// </summary>
/// <param name="trajectories"> The array of poses for the robot to execute</param>
private IEnumerator ExecuteTrajectories(RobotTrajectory trajectories)
{
// For every robot pose in trajectory plan
foreach (var point in trajectories.joint_trajectory.points)
{
var jointPositions = point.positions;
float[] result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray();
// Set the joint values for every joint
for (int joint = 0; joint < jointArticulationBodies.Length; joint++)
{
var joint1XDrive = jointArticulationBodies[joint].xDrive;
joint1XDrive.target = result[joint];
jointArticulationBodies[joint].xDrive = joint1XDrive;
}
// Wait for robot to achieve pose for all joint assignments
yield return(new WaitForSeconds(JOINT_ASSIGNMENT_WAIT));
}
}
public ExecuteKnownTrajectoryRequest(RobotTrajectory trajectory, bool wait_for_execution)
{
this.trajectory = trajectory;
this.wait_for_execution = wait_for_execution;
}
public ExecuteTrajectoryGoal(RobotTrajectory trajectory)
{
this.trajectory = trajectory;
}
public ExecuteTrajectoryGoal()
{
this.trajectory = new RobotTrajectory();
}
