/// <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();
}
