implementation of drecon in unity 2022 lts
forked from:
https://github.com/joanllobera/marathon-envs
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
89 lines
3.6 KiB
89 lines
3.6 KiB
9 months ago
|
using System.Collections;
|
||
|
|
using System.Collections.Generic;
|
||
|
|
using System.Linq;
|
||
|
|
using UnityEngine;
|
||
|
|
|
||
|
|
public class DebugArticulationBody : MonoBehaviour
|
||
|
|
{
|
||
|
|
public Vector3 jointAcceleration;
|
||
|
|
public Vector3 jointForce;
|
||
|
|
public Vector3 jointPosition;
|
||
|
|
public Vector3 jointVelocity;
|
||
|
|
public ArticulationJacobian jacobian;
|
||
|
|
public List<int> dofStartIndices;
|
||
|
|
public List<float> driveTargets;
|
||
|
|
public List<float> driveTargetVelocities;
|
||
|
|
public List<float> jointAccelerations;
|
||
|
|
public List<float> jointForces;
|
||
|
|
public List<float> jointPositions;
|
||
|
|
public List<float> jointVelocities;
|
||
|
|
ArticulationBody _articulationBody;
|
||
|
|
// Start is called before the first frame update
|
||
|
|
void Start()
|
||
|
|
{
|
||
|
|
_articulationBody = GetComponent<ArticulationBody>();
|
||
|
|
foreach (var ab in this.GetComponentsInChildren<ArticulationBody>())
|
||
|
|
{
|
||
|
|
if (ab.GetComponent<DebugArticulationBody>() == null)
|
||
|
|
{
|
||
|
|
ab.gameObject.AddComponent<DebugArticulationBody>();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
// Update is called once per frame
|
||
|
|
void Update()
|
||
|
|
{
|
||
|
|
if (_articulationBody == null)
|
||
|
|
return;
|
||
|
|
if (_articulationBody.isRoot)
|
||
|
|
{
|
||
|
|
if (dofStartIndices == null)
|
||
|
|
{
|
||
|
|
dofStartIndices = Enumerable.Range(0,_articulationBody.dofCount).ToList();
|
||
|
|
driveTargets = Enumerable.Range(0,_articulationBody.dofCount)
|
||
|
|
.Select(x=>0f).ToList();
|
||
|
|
driveTargetVelocities = Enumerable.Range(0,_articulationBody.dofCount)
|
||
|
|
.Select(x=>0f).ToList();
|
||
|
|
jointAccelerations = Enumerable.Range(0,_articulationBody.dofCount)
|
||
|
|
.Select(x=>0f).ToList();
|
||
|
|
jointForces = Enumerable.Range(0,_articulationBody.dofCount)
|
||
|
|
.Select(x=>0f).ToList();
|
||
|
|
jointPositions = Enumerable.Range(0,_articulationBody.dofCount)
|
||
|
|
.Select(x=>0f).ToList();
|
||
|
|
jointVelocities = Enumerable.Range(0,_articulationBody.dofCount)
|
||
|
|
.Select(x=>0f).ToList();
|
||
|
|
}
|
||
|
|
// _articulationBody.GetDenseJacobian(ref jacobian);
|
||
|
|
_articulationBody.GetDofStartIndices(dofStartIndices);
|
||
|
|
_articulationBody.GetDriveTargets(driveTargets);
|
||
|
|
_articulationBody.GetDriveTargetVelocities(driveTargetVelocities);
|
||
|
|
_articulationBody.GetJointAccelerations(jointAccelerations);
|
||
|
|
_articulationBody.GetJointForces(jointForces);
|
||
|
|
_articulationBody.GetJointPositions(jointPositions);
|
||
|
|
_articulationBody.GetJointVelocities(jointVelocities);
|
||
|
|
}
|
||
|
|
if (_articulationBody.dofCount > 0)
|
||
|
|
{
|
||
|
|
jointAcceleration.x = _articulationBody.jointAcceleration[0];
|
||
|
|
jointForce.x = _articulationBody.jointForce[0];
|
||
|
|
jointPosition.x = _articulationBody.jointPosition[0];
|
||
|
|
jointVelocity.x = _articulationBody.jointVelocity[0];
|
||
|
|
}
|
||
|
|
if (_articulationBody.dofCount > 1)
|
||
|
|
{
|
||
|
|
jointAcceleration.y = _articulationBody.jointAcceleration[1];
|
||
|
|
jointForce.y = _articulationBody.jointForce[1];
|
||
|
|
jointPosition.y = _articulationBody.jointPosition[1];
|
||
|
|
jointVelocity.y = _articulationBody.jointVelocity[1];
|
||
|
|
}
|
||
|
|
if (_articulationBody.dofCount > 2)
|
||
|
|
{
|
||
|
|
jointAcceleration.z = _articulationBody.jointAcceleration[2];
|
||
|
|
jointForce.z = _articulationBody.jointForce[2];
|
||
|
|
jointPosition.z = _articulationBody.jointPosition[2];
|
||
|
|
jointVelocity.z = _articulationBody.jointVelocity[2];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|