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 void DrawGhost(Drawing3d drawing, MultiDOFJointStateMsg message, Color color) { DrawGhost(drawing, GetJointPlacements(message), color); }