public void DrawJointPaths(Drawing3d drawing, JointTrajectoryMsg message, Color color, float pathThickness)
{
JointPlacement[][] jointPlacements = GetJointPlacements(message);
for (int Idx = 0; Idx < message.joint_names.Length; ++Idx)
{
DrawJointPath(drawing, jointPlacements, Idx, color, pathThickness);
}
}
// Returns a 2d array: JointPlacement[index of JointTrajectoryPoint][index of joint]
public JointPlacement[][] GetJointPlacements(JointTrajectoryMsg trajectory)
{
JointPlacement[][] result = new JointPlacement[trajectory.points.Length][];
for (int Idx = 0; Idx < trajectory.points.Length; ++Idx)
{
result[Idx] = GetJointPlacements(trajectory.points[Idx], trajectory.joint_names);
}
return(result);
}
public void DrawGhost(Drawing3d drawing, JointTrajectoryMsg message, int pointIndex, Color color)
{
DrawGhost(drawing, GetJointPlacements(message.points[pointIndex], message.joint_names), color);
}
public void DrawJointPath(Drawing3d drawing, JointTrajectoryMsg message, int jointIndex, Color color, float pathThickness)
{
DrawJointPath(drawing, GetJointPlacements(message), jointIndex, color, pathThickness);
}