/// <summary>
/// Execute the trajectory coming from MoveIt
/// </summary>
private IEnumerator ExecuteTrajectories(RosMessageTypes.Trajectory.JointTrajectory joint_trajectory)
{
// The joint order of MoveIt is not regular so we must read the names to map
// the right joint index.
string[] jointNamesFromROS = joint_trajectory.joint_names;
// For each trajectory returned by MoveIt set the xDrive and wait for completion
for (int jointConfigIndex = 0; jointConfigIndex < joint_trajectory.points.Length; jointConfigIndex++)
{
// Get the joint position in radians and transform to degrees
var jointPositions = joint_trajectory.points[jointConfigIndex].positions;
float[] result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray();
// No points
if (result.Length == 0)
{
Debug.Log("Joint trajectory returned 0 positions! Skipping");
continue;
}
// Set the joint positions into the xDrive of the ArticulationBody
for (int joint = 0; joint < result.Length - 1; joint++)
{
var jointName = jointNamesFromROS[joint];
var jointIndex = (int)jointPositionIndex[jointName];
var joint1XDrive = jointArticulationBodies[jointIndex].xDrive;
joint1XDrive.target = result[joint];
jointArticulationBodies[jointIndex].xDrive = joint1XDrive;
}
// Wait until the execution is complete
yield return(new WaitForSeconds(jointAssignmentWait));
}
}
/// <summary>
/// Start a separate routine which executes the trajectory
/// </summary>
public void TrajectoryHandler(RosMessageTypes.Trajectory.JointTrajectory response)
{
StartCoroutine(ExecuteTrajectories(response));
}
