void Update()
{
ArticulationReducedSpace a = articulation.jointAcceleration;
//print(a.dofCount);
print(a[0]);
}
void FixedUpdate()
{
var jPos = body.jointPosition;
jointPosition = new Vector3(jPos[0], jPos[1], jPos[2]);
jointPosition *= Mathf.Rad2Deg;
if (jointMode == JointMode.useDrive)
{
// Normalize Quaternion to suppress greater than 360 degree rotations
Quaternion normalizedRotation = Quaternion.Normalize(transform.rotation);
// Calculate drive targets
Vector3 driveTarget = new Vector3(
Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.x),
Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.y),
Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.z));
// Copy the X, Y, and Z target values into the drives...
ArticulationDrive xDrive = body.xDrive; xDrive.target = driveTarget.x; body.xDrive = xDrive;
ArticulationDrive yDrive = body.yDrive; yDrive.target = driveTarget.y; body.yDrive = yDrive;
ArticulationDrive zDrive = body.zDrive; zDrive.target = driveTarget.z; body.zDrive = zDrive;
}
else
{
Quaternion deltaRotation = Quaternion.Normalize(Quaternion.Inverse(body.transform.localRotation) * transform.rotation);
// Calculate drive velocity necessary to undo this delta in one fixed timestep
ArticulationReducedSpace driveTargetForce = new ArticulationReducedSpace(
((Mathf.DeltaAngle(0, deltaRotation.eulerAngles.x) * Mathf.Deg2Rad) / Time.fixedDeltaTime) * Strength,
((Mathf.DeltaAngle(0, deltaRotation.eulerAngles.y) * Mathf.Deg2Rad) / Time.fixedDeltaTime) * Strength,
((Mathf.DeltaAngle(0, deltaRotation.eulerAngles.z) * Mathf.Deg2Rad) / Time.fixedDeltaTime) * Strength);
// Apply the force in local space (unlike AddTorque which is global space)
// Ideally we'd use inverse dynamics or jointVelocity, but jointVelocity is bugged in 2020.1a22
if (jointMode == JointMode.useJointForce)
{
body.jointForce = driveTargetForce;
}
else if (jointMode == JointMode.useJointVelocity)
{
body.jointVelocity = driveTargetForce;
}
else if (jointMode == JointMode.useJointPosition)
{
Quaternion normalizedRotation = Quaternion.Normalize(transform.rotation);
ArticulationReducedSpace driveTarget = new ArticulationReducedSpace(
Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.x) * Mathf.Deg2Rad,
Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.y) * Mathf.Deg2Rad,
Mathf.DeltaAngle(0, normalizedRotation.eulerAngles.z) * Mathf.Deg2Rad);
body.jointPosition = driveTarget;
}
}
}
void setJointAcceleration(List <float> values)
{
List <float> current = new List <float>();
root.GetJointAccelerations(current);
for (int i = 0; i < current.Count; i++)
{
current[i] = values[i];
}
ArticulationReducedSpace acc = new ArticulationReducedSpace(0.0f);
root.SetJointAccelerations(values);
root.SetJointVelocities(values);
}
// CONTROL
public void ForceToRotation(float rotation)
{
// set target
RotateTo(rotation);
// force position
float rotationRads = Mathf.Deg2Rad * rotation;
ArticulationReducedSpace newPosition = new ArticulationReducedSpace(rotationRads);
articulation.jointPosition = newPosition;
// force velocity to zero
ArticulationReducedSpace newVelocity = new ArticulationReducedSpace(0.0f);
articulation.jointVelocity = newVelocity;
}
private void DrawArticulationReducedSpaceField(ArticulationReducedSpace ars, GUIContent label)
{
if (ars.dofCount == 0)
{
return;
}
if (ars.dofCount == 1)
{
EditorGUILayout.FloatField(label, ars[0]);
}
else if (ars.dofCount == 2)
{
EditorGUILayout.Vector2Field(label, new Vector2(ars[0], ars[1]));
}
else if (ars.dofCount == 3)
{
EditorGUILayout.Vector3Field(label, new Vector3(ars[0], ars[1], ars[2]));
}
}
