/// <summary> /// Initiates all JointStateControllers to update their joint angle based on the JointStateMessage. Indicate if it is a ROS-Message. /// </summary> /// <param name="jsc">List of <c>JointStateControllers</c></param> /// <param name="jsm">The <c>JointStateMessage</c> with updated paramters</param> /// <param name="ros">Bool indicating if it is a ros-message and additional steps have to be taken.</param> public void UpdateJointStates(JointStateController[] jsc, JointStateMsg jsm, bool ros) { for (int i = 0; i < jsc.Length; i++) { jsc[i].UpdateJoint(i, jsm, ros); } }
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); }
/// <summary> /// Update the joint state at which the <c>JointStateController</c> is attached based on the index /// </summary> /// <param name="index">Hierachical position of the joint</param> /// <param name="msg"><c>JointStateMsg</c> containing the information</param> /// <param name="isConvertingNecessary"> Indicate if a conversion from left to right-handed coordinate system is necessary</param> public void UpdateJoint(int index, JointStateMsg msg, bool isConvertingNecessary) { name = msg.name[index]; this.isConvertingNecessary = isConvertingNecessary; //angle = UnWrapAngle(msg.position[index]); angle = Mathf.Rad2Deg * msg.position[index]; velocity = msg.velocity[index]; effort = msg.effort[index]; targetState = (angle + angleOffset); }
public void DrawEffort(Drawing3d drawing, JointStateMsg message, Color color) { if (message.effort.Length > 0) { DrawEffort(drawing, GetJointPlacements(message), color, message.effort); } else { Debug.Log("This JointState message contains no Effort data!"); return; } }
public void DrawGhost(Drawing3d drawing, JointStateMsg message, Color color) { DrawGhost(drawing, GetJointPlacements(message), color); }
/// <summary> /// Updates the JointStatesMessage based on the known joint state controllers. /// </summary> void GetJointStates() { jointStateMsg = jointStateHandler.GetJointStateMsg(); }