internal void ApplyAction(MujocoJoint mJoint, float?target = null)
{
ConfigurableJoint configurableJoint = mJoint.Joint as ConfigurableJoint;
if (!target.HasValue) // handle random
{
target = UnityEngine.Random.value * 2 - 1;
}
var t = configurableJoint.targetAngularVelocity;
t.x = target.Value * mJoint.MaximumForce;
configurableJoint.targetAngularVelocity = t;
var angX = configurableJoint.angularXDrive;
angX.positionSpring = 1f;
var scale = mJoint.MaximumForce * Mathf.Pow(Mathf.Abs(target.Value), 3);
angX.positionDamper = Mathf.Max(1f, scale);
angX.maximumForce = Mathf.Max(1f, mJoint.MaximumForce);
configurableJoint.angularXDrive = angX;
}
static public void ApplyAction(MujocoJoint mJoint, float?target = null)
{
HingeJoint hingeJoint = mJoint.Joint as HingeJoint;
ConfigurableJoint configurableJoint = mJoint.Joint as ConfigurableJoint;
if (configurableJoint != null)
{
if (!target.HasValue) // handle random
{
target = Random.value * 2 - 1;
}
target = Mathf.Clamp(target.Value, -3f, 3f);
var t = configurableJoint.targetAngularVelocity;
t.x = target.Value * _velocityScaler;
configurableJoint.targetAngularVelocity = t;
var angX = configurableJoint.angularXDrive;
angX.positionSpring = 1f;
var scale = mJoint.MaximumForce * Mathf.Pow(Mathf.Abs(target.Value), 3);
angX.positionDamper = Mathf.Max(1f, scale);
angX.maximumForce = Mathf.Max(1f, scale);
configurableJoint.angularXDrive = angX;
return;
}
else if (hingeJoint == null)
{
return;
}
if (hingeJoint.useSpring)
{
var ctrlRangeMin = -1f;
var ctrlRangeMax = 1f;
// var ctrlRangeMin = 0f;
// var ctrlRangeMax = 1f;
var inputScale = ctrlRangeMax - ctrlRangeMin;
if (!target.HasValue) // handle random
{
target = ctrlRangeMin + (Random.value * inputScale);
}
var inputTarget = Mathf.Clamp(target.Value, ctrlRangeMin, ctrlRangeMax);
if (ctrlRangeMin < 0)
{
inputTarget = Mathf.Abs(ctrlRangeMin) + inputTarget;
}
else
{
inputTarget = inputTarget - Mathf.Abs(ctrlRangeMin);
}
inputTarget /= inputScale;
JointSpring js;
js = hingeJoint.spring;
var min = hingeJoint.limits.min;
var max = hingeJoint.limits.max;
var outputScale = max - min;
var outputTarget = min + (inputTarget * outputScale);
js.targetPosition = outputTarget;
hingeJoint.spring = js;
}
else if (hingeJoint.useMotor)
{
if (!target.HasValue) // handle random
{
target = Random.value * 2 - 1;
}
target = Mathf.Clamp(target.Value, -1f, 1f);
// target = Mathf.Clamp(target.Value, 0f, 1f);
// target *= 2;
// target -= 1f;
JointMotor jm;
jm = hingeJoint.motor;
jm.targetVelocity = target.Value * _velocityScaler;
hingeJoint.motor = jm;
}
}
