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])); } }