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); }
public void DrawGhost(Drawing3d drawing, JointTrajectoryPointMsg message, string[] jointNames, Color color) { DrawGhost(drawing, GetJointPlacements(message, jointNames), color); }