public JointPlacement[] GetJointPlacements(MultiDOFJointStateMsg jointState)
{
Quaternion lastWorldRotation = m_Robot.transform.rotation;
Vector3 lastWorldPosition = m_Robot.transform.position;
GameObject lastJoint = m_Robot.gameObject;
JointPlacement[] result = new JointPlacement[jointState.joint_names.Length];
for (int i = 0; i < jointState.joint_names.Length; ++i)
{
JointPlacement jointData = m_JointsByName[jointState.joint_names[i]];
ArticulationBody body = jointData.Joint.GetComponent <ArticulationBody>();
Quaternion jointRotation = body.anchorRotation * jointState.transforms[i].rotation.From <FLU>() * Quaternion.Inverse(body.anchorRotation);
Quaternion localRotation = jointData.Rotation * jointRotation;
Vector3 localPosition = Vector3.Scale(lastJoint.transform.InverseTransformPoint(body.transform.position), jointState.transforms[i].translation.From <FLU>());
Vector3 worldPosition = lastWorldPosition + lastWorldRotation * (lastJoint.transform.InverseTransformPoint(body.transform.position) + localPosition);
Quaternion worldRotation = lastWorldRotation * localRotation;
result[i] = new JointPlacement {
Joint = jointData.Joint, Position = worldPosition, Rotation = worldRotation
};
lastWorldPosition = worldPosition;
lastWorldRotation = worldRotation;
lastJoint = body.gameObject;
}
return(result);
}
public JointPlacement[] GetJointPlacements(JointStateMsg jointState)
{
Quaternion lastWorldRotation = m_Robot.transform.rotation;
Vector3 lastWorldPosition = m_Robot.transform.position;
GameObject lastJoint = m_Robot.gameObject;
JointPlacement[] result = new JointPlacement[jointState.name.Length];
for (int i = 0; i < jointState.name.Length; ++i)
{
JointPlacement jointData = m_JointsByName[jointState.name[i]];
float rotationDegrees = (float)(jointState.position[i] * Mathf.Rad2Deg);
ArticulationBody body = jointData.Joint.GetComponent <ArticulationBody>();
Quaternion jointRotation = body.anchorRotation * Quaternion.Euler(rotationDegrees, 0, 0) * Quaternion.Inverse(body.anchorRotation);
Quaternion localRotation = jointData.Rotation * jointRotation;
Vector3 localPosition = lastJoint.transform.InverseTransformPoint(body.transform.position);
Vector3 worldPosition = lastWorldPosition + lastWorldRotation * localPosition;
Quaternion worldRotation = lastWorldRotation * localRotation;
result[i] = new JointPlacement {
Joint = jointData.Joint, Position = worldPosition, Rotation = worldRotation
};
lastWorldPosition = worldPosition;
lastWorldRotation = worldRotation;
lastJoint = body.gameObject;
}
return(result);
}
public JointPlacement[] GetJointPlacements(JointTrajectoryPointMsg point, string[] jointNames)
{
Quaternion lastWorldRotation = m_Robot.transform.rotation;
Vector3 lastWorldPosition = m_Robot.transform.position;
GameObject lastJoint = m_Robot.gameObject;
JointPlacement[] result = new JointPlacement[jointNames.Length];
for (int Idx = 0; Idx < point.positions.Length; ++Idx)
{
JointPlacement jointData = m_JointsByName[jointNames[Idx]];
float rotationDegrees = (float)(point.positions[Idx] * Mathf.Rad2Deg);
ArticulationBody body = jointData.Joint.GetComponent <ArticulationBody>();
Quaternion jointRotation = body.anchorRotation * Quaternion.Euler(rotationDegrees, 0, 0) * Quaternion.Inverse(body.anchorRotation);
Quaternion localRotation = jointData.Rotation * jointRotation;
Vector3 localPosition = lastJoint.transform.InverseTransformPoint(body.transform.position);
Vector3 worldPosition = lastWorldPosition + lastWorldRotation * localPosition;
Quaternion worldRotation = lastWorldRotation * localRotation;
result[Idx] = new JointPlacement {
Joint = jointData.Joint, Position = worldPosition, Rotation = worldRotation
};
lastWorldPosition = worldPosition;
lastWorldRotation = worldRotation;
lastJoint = body.gameObject;
}
return(result);
}
// 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);
}
