8 changed files with 1301 additions and 202 deletions
@ -0,0 +1,152 @@ |
|||||
|
using System.Collections; |
||||
|
using System.Collections.Generic; |
||||
|
using UnityEngine; |
||||
|
|
||||
|
[System.Serializable] |
||||
|
public class BodyPart |
||||
|
{ |
||||
|
[Header("Body Part Info")] [Space(10)] public ConfigurableJoint joint; |
||||
|
public Rigidbody rb; |
||||
|
[HideInInspector] public Vector3 startingPos; |
||||
|
[HideInInspector] public Quaternion startingRot; |
||||
|
[HideInInspector] public JointDriveController thisJdController; |
||||
|
|
||||
|
[Header("Current Joint Settings")] |
||||
|
[Space(10)] |
||||
|
public Vector3 currentEularJointRotation; |
||||
|
|
||||
|
[HideInInspector] public float currentStrength; |
||||
|
public float currentXNormalizedRot; |
||||
|
public float currentYNormalizedRot; |
||||
|
public float currentZNormalizedRot; |
||||
|
|
||||
|
[Header("Other Debug Info")] |
||||
|
[Space(10)] |
||||
|
public Vector3 currentJointForce; |
||||
|
|
||||
|
public float currentJointForceSqrMag; |
||||
|
public Vector3 currentJointTorque; |
||||
|
public float currentJointTorqueSqrMag; |
||||
|
public AnimationCurve jointForceCurve = new AnimationCurve(); |
||||
|
public AnimationCurve jointTorqueCurve = new AnimationCurve(); |
||||
|
|
||||
|
/// <summary>
|
||||
|
/// Reset body part to initial configuration.
|
||||
|
/// </summary>
|
||||
|
public void Reset(BodyPart bp) |
||||
|
{ |
||||
|
bp.rb.transform.position = bp.startingPos; |
||||
|
bp.rb.transform.rotation = bp.startingRot; |
||||
|
bp.rb.velocity = Vector3.zero; |
||||
|
bp.rb.angularVelocity = Vector3.zero; |
||||
|
} |
||||
|
|
||||
|
/// <summary>
|
||||
|
/// Apply torque according to defined goal `x, y, z` angle and force `strength`.
|
||||
|
/// </summary>
|
||||
|
public void SetJointTargetRotation(float x, float y, float z) |
||||
|
{ |
||||
|
x = (x + 1f) * 0.5f; |
||||
|
y = (y + 1f) * 0.5f; |
||||
|
z = (z + 1f) * 0.5f; |
||||
|
|
||||
|
var xRot = Mathf.Lerp(joint.lowAngularXLimit.limit, joint.highAngularXLimit.limit, x); |
||||
|
var yRot = Mathf.Lerp(-joint.angularYLimit.limit, joint.angularYLimit.limit, y); |
||||
|
var zRot = Mathf.Lerp(-joint.angularZLimit.limit, joint.angularZLimit.limit, z); |
||||
|
|
||||
|
currentXNormalizedRot = |
||||
|
Mathf.InverseLerp(joint.lowAngularXLimit.limit, joint.highAngularXLimit.limit, xRot); |
||||
|
currentYNormalizedRot = Mathf.InverseLerp(-joint.angularYLimit.limit, joint.angularYLimit.limit, yRot); |
||||
|
currentZNormalizedRot = Mathf.InverseLerp(-joint.angularZLimit.limit, joint.angularZLimit.limit, zRot); |
||||
|
|
||||
|
joint.targetRotation = Quaternion.Euler(xRot, yRot, zRot); |
||||
|
currentEularJointRotation = new Vector3(xRot, yRot, zRot); |
||||
|
} |
||||
|
|
||||
|
public void SetJointStrength(float strength) |
||||
|
{ |
||||
|
var rawVal = (strength + 1f) * 0.5f * thisJdController.maxJointForceLimit; |
||||
|
var jd = new JointDrive |
||||
|
{ |
||||
|
positionSpring = thisJdController.maxJointSpring, |
||||
|
positionDamper = thisJdController.jointDampen, |
||||
|
maximumForce = rawVal |
||||
|
}; |
||||
|
joint.slerpDrive = jd; |
||||
|
currentStrength = jd.maximumForce; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
public class JointDriveController : MonoBehaviour |
||||
|
{ |
||||
|
[Header("Joint Drive Settings")] |
||||
|
[Space(10)] |
||||
|
public float maxJointSpring; |
||||
|
|
||||
|
public float jointDampen; |
||||
|
public float maxJointForceLimit; |
||||
|
|
||||
|
[HideInInspector] public Dictionary<Transform, BodyPart> bodyPartsDict = new Dictionary<Transform, BodyPart>(); |
||||
|
|
||||
|
[HideInInspector] public List<BodyPart> bodyPartsList = new List<BodyPart>(); |
||||
|
const float k_MaxAngularVelocity = 50.0f; |
||||
|
|
||||
|
/// <summary>
|
||||
|
/// Create BodyPart object and add it to dictionary.
|
||||
|
/// </summary>
|
||||
|
public void SetupBodyPart(Transform t) |
||||
|
{ |
||||
|
var bp = new BodyPart |
||||
|
{ |
||||
|
rb = t.GetComponent<Rigidbody>(), |
||||
|
joint = t.GetComponent<ConfigurableJoint>(), |
||||
|
startingPos = t.position, |
||||
|
startingRot = t.rotation |
||||
|
}; |
||||
|
bp.rb.maxAngularVelocity = k_MaxAngularVelocity; |
||||
|
|
||||
|
if (bp.joint) |
||||
|
{ |
||||
|
var jd = new JointDrive |
||||
|
{ |
||||
|
positionSpring = maxJointSpring, |
||||
|
positionDamper = jointDampen, |
||||
|
maximumForce = maxJointForceLimit |
||||
|
}; |
||||
|
bp.joint.slerpDrive = jd; |
||||
|
} |
||||
|
|
||||
|
bp.thisJdController = this; |
||||
|
bodyPartsDict.Add(t, bp); |
||||
|
bodyPartsList.Add(bp); |
||||
|
} |
||||
|
|
||||
|
public void GetCurrentJointForces() |
||||
|
{ |
||||
|
foreach (var bodyPart in bodyPartsDict.Values) |
||||
|
{ |
||||
|
if (bodyPart.joint) |
||||
|
{ |
||||
|
bodyPart.currentJointForce = bodyPart.joint.currentForce; |
||||
|
bodyPart.currentJointForceSqrMag = bodyPart.joint.currentForce.magnitude; |
||||
|
bodyPart.currentJointTorque = bodyPart.joint.currentTorque; |
||||
|
bodyPart.currentJointTorqueSqrMag = bodyPart.joint.currentTorque.magnitude; |
||||
|
if (Application.isEditor) |
||||
|
{ |
||||
|
if (bodyPart.jointForceCurve.length > 1000) |
||||
|
{ |
||||
|
bodyPart.jointForceCurve = new AnimationCurve(); |
||||
|
} |
||||
|
|
||||
|
if (bodyPart.jointTorqueCurve.length > 1000) |
||||
|
{ |
||||
|
bodyPart.jointTorqueCurve = new AnimationCurve(); |
||||
|
} |
||||
|
|
||||
|
bodyPart.jointForceCurve.AddKey(Time.time, bodyPart.currentJointForceSqrMag); |
||||
|
bodyPart.jointTorqueCurve.AddKey(Time.time, bodyPart.currentJointTorqueSqrMag); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
} |
||||
@ -0,0 +1,11 @@ |
|||||
|
fileFormatVersion: 2 |
||||
|
guid: e109baaf62426244eb24aec54decdf71 |
||||
|
MonoImporter: |
||||
|
externalObjects: {} |
||||
|
serializedVersion: 2 |
||||
|
defaultReferences: [] |
||||
|
executionOrder: 0 |
||||
|
icon: {instanceID: 0} |
||||
|
userData: |
||||
|
assetBundleName: |
||||
|
assetBundleVariant: |
||||
File diff suppressed because it is too large
@ -1,5 +1,5 @@ |
|||||
fileFormatVersion: 2 |
fileFormatVersion: 2 |
||||
guid: feef188ff5146fc40805e1c2d1015a4f |
guid: 7fe3405e35529f240b5beac3990ab3d3 |
||||
PrefabImporter: |
PrefabImporter: |
||||
externalObjects: {} |
externalObjects: {} |
||||
userData: |
userData: |
||||
@ -1 +1 @@ |
|||||
{"count":1,"self":35.984704,"total":37.310626,"children":{"InitializeActuators":{"count":1,"self":0.001116,"total":0.001116,"children":null},"InitializeSensors":{"count":1,"self":0.002,"total":0.002,"children":null},"AgentSendState":{"count":1851,"self":0.0135468,"total":0.40610009999999996,"children":{"CollectObservations":{"count":926,"self":0.36748139999999996,"total":0.36748139999999996,"children":null},"WriteActionMask":{"count":926,"self":0.0012209,"total":0.0012209,"children":null},"RequestDecision":{"count":926,"self":0.023850999999999997,"total":0.023850999999999997,"children":null}}},"DecideAction":{"count":1851,"self":0.6228504,"total":0.6228504,"children":null},"AgentAct":{"count":1851,"self":0.29285659999999997,"total":0.29285659999999997,"children":null}},"gauges":{"DReCon-v0.CumulativeReward":{"count":1,"max":67.6433258,"min":67.6433258,"runningAverage":67.6433258,"value":67.6433258,"weightedAverage":67.6433258}},"metadata":{"timer_format_version":"0.1.0","start_time_seconds":"1712920398","unity_version":"2022.3.5f1","command_line_arguments":"C:\\Program Files\\Unity\\Hub\\Editor\\2022.3.5f1\\Editor\\Unity.exe -projectpath C:\\Users\\caile\\Desktop\\drecon-unity -useHub -hubIPC -cloudEnvironment production -licensingIpc LicenseClient-cail -hubSessionId 3fbdf177-9b94-4cc1-adfd-c08277a8eede -accessToken odeT9cSeLBQY3FrA1yA0599oEegUBhCWJkhIJG2gUmk005f","communication_protocol_version":"1.5.0","com.unity.ml-agents_version":"2.0.1","scene_name":"DreconDemo","end_time_seconds":"1712920435"}} |
{"count":1,"self":169.173824,"total":175.70866619999998,"children":{"InitializeActuators":{"count":1,"self":0.0017232999999999999,"total":0.0017232999999999999,"children":null},"InitializeSensors":{"count":1,"self":0.0015046,"total":0.0015046,"children":null},"AgentSendState":{"count":9952,"self":0.0856932,"total":2.6710059999999998,"children":{"CollectObservations":{"count":4976,"self":2.546122,"total":2.546122,"children":null},"WriteActionMask":{"count":4976,"self":0.0101377,"total":0.0101377,"children":null},"RequestDecision":{"count":4976,"self":0.0290531,"total":0.0290531,"children":null}}},"DecideAction":{"count":9952,"self":2.6619052,"total":2.6619051,"children":null},"AgentAct":{"count":9952,"self":1.1982021,"total":1.1982021,"children":null}},"gauges":{"DReCon-v0.CumulativeReward":{"count":2,"max":485.7487,"min":60.126297,"runningAverage":272.9375,"value":485.7487,"weightedAverage":166.531891}},"metadata":{"timer_format_version":"0.1.0","start_time_seconds":"1712925795","unity_version":"2022.3.5f1","command_line_arguments":"C:\\Program Files\\Unity\\Hub\\Editor\\2022.3.5f1\\Editor\\Unity.exe -projectpath C:\\Users\\caile\\Desktop\\drecon-unity -useHub -hubIPC -cloudEnvironment production -licensingIpc LicenseClient-cail -hubSessionId 3fbdf177-9b94-4cc1-adfd-c08277a8eede -accessToken odeT9cSeLBQY3FrA1yA0599oEegUBhCWJkhIJG2gUmk005f","communication_protocol_version":"1.5.0","com.unity.ml-agents_version":"2.0.1","scene_name":"DreconDemo","end_time_seconds":"1712925970"}} |
||||
Loading…
Reference in new issue