private void DrawFreePrismatic(ArticulationBody body, Vector3 primaryAxis, Vector3 anchorPosition, Quaternion anchorRotation)
{
Vector3 lp, up;
float paddingAmount = 15;
Handles.color = new Color(1, 1, 1, DashAlpha);
Vector3 primaryAxisRotated = anchorRotation * primaryAxis;
Vector3 padding = primaryAxisRotated * paddingAmount;
// If not in play mode and match anchors is off, use the anchor position
if (!body.matchAnchors & !EditorApplication.isPlaying)
{
lp = anchorPosition - padding;
up = anchorPosition + padding;
}
// If in play mode or match anchors is on, calculate the correct position
else
{
Vector3 bodyPosOnPrimaryAxis = Vector3.Project(body.transform.position - anchorPosition, primaryAxisRotated);
lp = anchorPosition + bodyPosOnPrimaryAxis - padding;
up = anchorPosition + bodyPosOnPrimaryAxis + padding;
}
Handles.DrawDottedLine(lp, up, DashSize);
}
private void ShowSphericalLimits(JointAngularLimitHandle handle, ArticulationBody body, Matrix4x4 parentAnchorSpace)
{
using (new Handles.DrawingScope(parentAnchorSpace))
{
handle.xMin = body.xDrive.lowerLimit;
handle.xMax = body.xDrive.upperLimit;
handle.yMin = body.yDrive.lowerLimit;
handle.yMax = body.yDrive.upperLimit;
handle.zMin = body.zDrive.lowerLimit;
handle.zMax = body.zDrive.upperLimit;
EditorGUI.BeginChangeCheck();
handle.radius = HandleUtility.GetHandleSize(Vector3.zero);
handle.DrawHandle(true);
if (EditorGUI.EndChangeCheck())
{
Undo.RecordObject(target, "Changing Articulation body parent anchor rotation limits");
body.xDrive = SetDriveLimits(body.xDrive, handle.xMin, handle.xMax);
body.yDrive = SetDriveLimits(body.yDrive, handle.yMin, handle.yMax);
body.zDrive = SetDriveLimits(body.zDrive, handle.zMin, handle.zMax);
}
}
}
//Quaternion currentRotation;
private void Awake()
{
joint = GetComponent <HingeJoint>();
articulation = GetComponent <ArticulationBody>();
defaultRotation = transform.localRotation;
}
private void DisplayProperAnchorHandle(ArticulationBody body, ref Vector3 anchorPos, ref Quaternion anchorRot)
{
float handleScaling = 0.5f;
var bodySpace = Matrix4x4.TRS(body.transform.position, body.transform.rotation, Vector3.one * handleScaling);
// Need to pre-scale in body space because our handle matrix is scaled, we will remove scaling after reading back
anchorPos *= (1 / handleScaling);
using (new Handles.DrawingScope(bodySpace))
{
if (Tools.current == Tool.Move)
{
anchorPos = Handles.PositionHandle(anchorPos, anchorRot);
}
if (Tools.current == Tool.Rotate)
{
anchorRot = Handles.RotationHandle(anchorRot, anchorPos);
}
if (Tools.current == Tool.Transform)
{
Handles.TransformHandle(ref anchorPos, ref anchorRot);
}
}
// Don't forget to remove scaling
anchorPos *= handleScaling;
}
private void ShowPrismaticLimits(ArticulationBody body, Matrix4x4 parentAnchorSpace, Vector3 anchorPosition, Quaternion anchorRotation)
{
Vector3 primaryAxis = Vector3.zero;
// compute the primary axis of the prismatic
ArticulationDrive drive = body.xDrive;
if (body.linearLockX != ArticulationDofLock.LockedMotion)
{
primaryAxis = Vector3.right;
drive = body.xDrive;
}
else if (body.linearLockY != ArticulationDofLock.LockedMotion)
{
primaryAxis = Vector3.up;
drive = body.yDrive;
}
else if (body.linearLockZ != ArticulationDofLock.LockedMotion)
{
primaryAxis = Vector3.forward;
drive = body.zDrive;
}
if (body.linearLockX == ArticulationDofLock.FreeMotion || body.linearLockY == ArticulationDofLock.FreeMotion || body.linearLockZ == ArticulationDofLock.FreeMotion)
{
DrawFreePrismatic(body, primaryAxis, anchorPosition, anchorRotation);
return;
}
DrawLimitedPrismatic(body, primaryAxis, drive, parentAnchorSpace);
}
public JointPlacement[] GetJointPlacements(MultiDOFJointStateMsg jointState)
{
Quaternion lastWorldRotation = m_Robot.transform.rotation;
Vector3 lastWorldPosition = m_Robot.transform.position;
GameObject lastJoint = m_Robot.gameObject;
JointPlacement[] result = new JointPlacement[jointState.joint_names.Length];
for (int i = 0; i < jointState.joint_names.Length; ++i)
{
JointPlacement jointData = m_JointsByName[jointState.joint_names[i]];
ArticulationBody body = jointData.Joint.GetComponent <ArticulationBody>();
Quaternion jointRotation = body.anchorRotation * jointState.transforms[i].rotation.From <FLU>() * Quaternion.Inverse(body.anchorRotation);
Quaternion localRotation = jointData.Rotation * jointRotation;
Vector3 localPosition = Vector3.Scale(lastJoint.transform.InverseTransformPoint(body.transform.position), jointState.transforms[i].translation.From <FLU>());
Vector3 worldPosition = lastWorldPosition + lastWorldRotation * (lastJoint.transform.InverseTransformPoint(body.transform.position) + localPosition);
Quaternion worldRotation = lastWorldRotation * localRotation;
result[i] = new JointPlacement {
Joint = jointData.Joint, Position = worldPosition, Rotation = worldRotation
};
lastWorldPosition = worldPosition;
lastWorldRotation = worldRotation;
lastJoint = body.gameObject;
}
return(result);
}
public JointPlacement[] GetJointPlacements(JointTrajectoryPointMsg point, string[] jointNames)
{
Quaternion lastWorldRotation = m_Robot.transform.rotation;
Vector3 lastWorldPosition = m_Robot.transform.position;
GameObject lastJoint = m_Robot.gameObject;
JointPlacement[] result = new JointPlacement[jointNames.Length];
for (int Idx = 0; Idx < point.positions.Length; ++Idx)
{
JointPlacement jointData = m_JointsByName[jointNames[Idx]];
float rotationDegrees = (float)(point.positions[Idx] * Mathf.Rad2Deg);
ArticulationBody body = jointData.Joint.GetComponent <ArticulationBody>();
Quaternion jointRotation = body.anchorRotation * Quaternion.Euler(rotationDegrees, 0, 0) * Quaternion.Inverse(body.anchorRotation);
Quaternion localRotation = jointData.Rotation * jointRotation;
Vector3 localPosition = lastJoint.transform.InverseTransformPoint(body.transform.position);
Vector3 worldPosition = lastWorldPosition + lastWorldRotation * localPosition;
Quaternion worldRotation = lastWorldRotation * localRotation;
result[Idx] = new JointPlacement {
Joint = jointData.Joint, Position = worldPosition, Rotation = worldRotation
};
lastWorldPosition = worldPosition;
lastWorldRotation = worldRotation;
lastJoint = body.gameObject;
}
return(result);
}
private void DisplayJointLimits(ArticulationBody body)
{
ArticulationBody parentBody = body.transform.parent.GetComponentInParent <ArticulationBody>();
Matrix4x4 parentAnchorSpace = Matrix4x4.TRS(parentBody.transform.TransformPoint(body.parentAnchorPosition), parentBody.transform.rotation * body.parentAnchorRotation, Vector3.one);
// this is root body no joint limits
if (parentBody == null)
{
return;
}
if (body.jointType == ArticulationJointType.PrismaticJoint)
{
ShowPrismaticLimits(body, parentBody, parentAnchorSpace);
return;
}
if (body.jointType == ArticulationJointType.RevoluteJoint)
{
ShowRevoluteLimits(body, parentBody, parentAnchorSpace);
return;
}
if (body.jointType == ArticulationJointType.SphericalJoint)
{
ShowSphericalLimits(body, parentBody, parentAnchorSpace);
return;
}
}
public JointPlacement[] GetJointPlacements(JointStateMsg jointState)
{
Quaternion lastWorldRotation = m_Robot.transform.rotation;
Vector3 lastWorldPosition = m_Robot.transform.position;
GameObject lastJoint = m_Robot.gameObject;
JointPlacement[] result = new JointPlacement[jointState.name.Length];
for (int i = 0; i < jointState.name.Length; ++i)
{
JointPlacement jointData = m_JointsByName[jointState.name[i]];
float rotationDegrees = (float)(jointState.position[i] * Mathf.Rad2Deg);
ArticulationBody body = jointData.Joint.GetComponent <ArticulationBody>();
Quaternion jointRotation = body.anchorRotation * Quaternion.Euler(rotationDegrees, 0, 0) * Quaternion.Inverse(body.anchorRotation);
Quaternion localRotation = jointData.Rotation * jointRotation;
Vector3 localPosition = lastJoint.transform.InverseTransformPoint(body.transform.position);
Vector3 worldPosition = lastWorldPosition + lastWorldRotation * localPosition;
Quaternion worldRotation = lastWorldRotation * localRotation;
result[i] = new JointPlacement {
Joint = jointData.Joint, Position = worldPosition, Rotation = worldRotation
};
lastWorldPosition = worldPosition;
lastWorldRotation = worldRotation;
lastJoint = body.gameObject;
}
return(result);
}
void Start()
{
roboState = Resources.Load <StateObject>("StateObject");
articulation = GetComponent <ArticulationBody>();
minRot = articulation.xDrive.lowerLimit;
maxRot = articulation.xDrive.upperLimit;
}
public PhysicsBodySensor(ArticulationBody rootBody, PhysicsSensorSettings settings, string sensorName = null)
{
var poseExtractor = new ArticulationBodyPoseExtractor(rootBody);
m_PoseExtractor = poseExtractor;
m_SensorName = string.IsNullOrEmpty(sensorName) ? $"ArticulationBodySensor:{rootBody?.name}" : sensorName;
m_Settings = settings;
var numJointExtractorObservations = 0;
var articBodies = poseExtractor.Bodies;
if (articBodies != null)
{
m_JointExtractors = new IJointExtractor[articBodies.Length - 1]; // skip the root
for (var i = 1; i < articBodies.Length; i++)
{
var jointExtractor = new ArticulationBodyJointExtractor(articBodies[i]);
numJointExtractorObservations += jointExtractor.NumObservations(settings);
m_JointExtractors[i - 1] = jointExtractor;
}
}
else
{
m_JointExtractors = new IJointExtractor[0];
}
var numTransformObservations = m_PoseExtractor.GetNumPoseObservations(settings);
m_Shape = new[] { numTransformObservations + numJointExtractorObservations };
}
protected override void ImportJointData(Joint joint)
{
#if UNITY_2020_1_OR_NEWER
AdjustMovement(joint);
if (joint.dynamics != null)
{
unityJoint.linearDamping = (float)joint.dynamics.damping;
unityJoint.jointFriction = (float)joint.dynamics.friction;
}
else
{
unityJoint.angularDamping = 0;
unityJoint.jointFriction = 0;
}
#else
ArticulationBody prismaticJoint = (ArticulationBody)unityJoint;
prismaticJoint.axis = (joint.axis != null) ? GetAxis(joint.axis) : GetDefaultAxis();
if (joint.dynamics != null)
{
prismaticJoint.xDrive = GetJointDrive(joint.dynamics);
}
if (joint.limit != null)
{
PrismaticJointLimitsManager prismaticLimits = GetComponent <PrismaticJointLimitsManager>();
prismaticLimits.InitializeLimits(joint.limit);
}
#endif
}
public void ExportSpecificJointData_Succeeds()
{
GameObject linkObject = new GameObject("link");
TestUrdfJointRevolute urdfJoint = linkObject.AddComponent <TestUrdfJointRevolute>();
ArticulationBody articulationBody = linkObject.GetComponent <ArticulationBody>();
urdfJoint.SetAxisOfMotion(new Vector3(1.2345678f, 2.3456789f, 3.4567891f));
urdfJoint.Dynamics(new Joint.Dynamics(4, 5));
var joint = new Joint(
name: "custom_joint", type: "continuous", parent: "base", child: "link");
urdfJoint.TestExportSpecificJointData(joint);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.234568f, (float)joint.axis.xyz[0]);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(2.345679f, (float)joint.axis.xyz[1]);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(3.456789f, (float)joint.axis.xyz[2]);
Assert.AreEqual(4, joint.dynamics.damping);
Assert.AreEqual(5, joint.dynamics.friction);
Assert.AreEqual(articulationBody.xDrive.lowerLimit * Mathf.Deg2Rad, joint.limit.lower);
Assert.AreEqual(articulationBody.xDrive.upperLimit * Mathf.Deg2Rad, joint.limit.upper);
Assert.AreEqual(articulationBody.xDrive.forceLimit, joint.limit.effort);
Assert.AreEqual(articulationBody.maxAngularVelocity, joint.limit.velocity);
Object.DestroyImmediate(linkObject);
}
private void DrawArticulationBodyInfo(ArticulationBody body)
{
EditorGUI.indentLevel++;
EditorGUILayout.FloatField(Style.infoSpeed, body.velocity.magnitude);
EditorGUILayout.Vector3Field(Style.infoVel, body.velocity);
EditorGUILayout.Vector3Field(Style.infoAngVel, body.angularVelocity);
EditorGUILayout.Vector3Field(Style.infoInertiaTensor, body.inertiaTensor);
EditorGUILayout.Vector3Field(Style.infoInertiaTensorRotation, body.inertiaTensorRotation.eulerAngles);
EditorGUILayout.Vector3Field(Style.infoLocalCenterOfMass, body.centerOfMass);
EditorGUILayout.Vector3Field(Style.infoWorldCenterOfMass, body.worldCenterOfMass);
EditorGUILayout.LabelField(Style.infoSleepState, body.IsSleeping() ? Style.sleep : Style.awake);
EditorGUILayout.FloatField(Style.infoSleepThreshold, body.sleepThreshold);
EditorGUILayout.FloatField(Style.infoMaxLinVel, body.maxLinearVelocity);
EditorGUILayout.FloatField(Style.infoMaxAngVel, body.maxAngularVelocity);
EditorGUILayout.IntField(Style.infoSolverIterations, body.solverIterations);
EditorGUILayout.IntField(Style.infoSolverVelIterations, body.solverVelocityIterations);
EditorGUILayout.IntField(Style.infoBodyIndex, body.index);
if (!body.isRoot && body.jointType != ArticulationJointType.FixedJoint)
{
m_ShowFolduot = EditorGUILayout.Foldout(m_ShowFolduot, Style.infoJointInfo);
if (m_ShowFolduot)
{
DrawArticulationReducedSpaceField(body.jointPosition, Style.infoJointPosition);
DrawArticulationReducedSpaceField(body.jointVelocity, Style.infoJointVelocity);
DrawArticulationReducedSpaceField(body.jointForce, Style.infoJointForce);
DrawArticulationReducedSpaceField(body.jointAcceleration, Style.infoJointAcceleration);
}
}
EditorGUI.indentLevel--;
}
public void ImportJointData_SpecificLimit_Succeeds()
{
var joint = new Joint(
name: "custom_joint", type: "planar", parent: "base", child: "link",
limit: new Joint.Limit(4, 5, 6, 7),
dynamics: new Joint.Dynamics(8, 9));
GameObject baseObject = new GameObject("base");
GameObject linkObject = new GameObject("link");
linkObject.transform.parent = baseObject.transform;
UrdfJoint.Create(baseObject, UrdfJoint.JointTypes.Fixed);
TestUrdfJointPlanar urdfJoint = linkObject.AddComponent <TestUrdfJointPlanar>();
ArticulationBody articulationBody = linkObject.GetComponent <ArticulationBody>();
urdfJoint.TestImportJointData(joint);
Assert.AreEqual(ArticulationDofLock.LockedMotion, articulationBody.linearLockX);
Assert.AreEqual(ArticulationDofLock.LimitedMotion, articulationBody.linearLockY);
Assert.AreEqual(ArticulationDofLock.LimitedMotion, articulationBody.linearLockZ);
Assert.AreEqual(4, articulationBody.xDrive.lowerLimit);
Assert.AreEqual(4, articulationBody.yDrive.lowerLimit);
Assert.AreEqual(4, articulationBody.zDrive.lowerLimit);
Assert.AreEqual(5, articulationBody.xDrive.upperLimit);
Assert.AreEqual(5, articulationBody.yDrive.upperLimit);
Assert.AreEqual(5, articulationBody.zDrive.upperLimit);
Assert.AreEqual(6, articulationBody.xDrive.forceLimit);
Assert.AreEqual(6, articulationBody.yDrive.forceLimit);
Assert.AreEqual(6, articulationBody.zDrive.forceLimit);
Assert.AreEqual(7, articulationBody.maxLinearVelocity);
Object.DestroyImmediate(baseObject);
}
public static void Create(GameObject linkObject, Link.Inertial inertial = null)
{
UrdfInertial urdfInertial = linkObject.AddComponent <UrdfInertial>();
#if UNITY_2020_1_OR_NEWER
ArticulationBody robotLink = urdfInertial.GetComponent <ArticulationBody>();
#else
Rigidbody robotLink = urdfInertial.GetComponent <Rigidbody>();
#endif
if (inertial != null)
{
robotLink.mass = ((float)inertial.mass > 0)?((float)inertial.mass):minMass;
if (inertial.origin != null)
{
robotLink.centerOfMass = UrdfOrigin.GetPositionFromUrdf(inertial.origin);
}
else
{
robotLink.centerOfMass = Vector3.zero;
}
urdfInertial.ImportInertiaData(inertial);
urdfInertial.useUrdfData = true;
}
urdfInertial.displayInertiaGizmo = false;
}
private void ImportInertiaData(Link.Inertial inertial)
{
Vector3 eigenvalues;
Vector3[] eigenvectors;
Matrix3x3 rotationMatrix = ToMatrix3x3(inertial.inertia);
rotationMatrix.DiagonalizeRealSymmetric(out eigenvalues, out eigenvectors);
#if UNITY_2020_1_OR_NEWER
ArticulationBody robotLink = GetComponent <ArticulationBody>();
#else
Rigidbody robotLink = GetComponent <Rigidbody>();
#endif
Vector3 inertiaEulerAngles;
if (inertial.origin != null)
{
inertiaEulerAngles = UrdfOrigin.GetRotationFromUrdf(inertial.origin);
}
else
{
inertiaEulerAngles = new Vector3(0, 0, 0);
}
this.inertialAxisRotation.eulerAngles = inertiaEulerAngles;
robotLink.inertiaTensor = ToUnityInertiaTensor(FixMinInertia(eigenvalues));
robotLink.inertiaTensorRotation = ToQuaternion(eigenvectors[0], eigenvectors[1], eigenvectors[2]).Ros2Unity() * this.inertialAxisRotation;
this.centerOfMass = robotLink.centerOfMass;
this.inertiaTensor = robotLink.inertiaTensor;
this.inertiaTensorRotation = robotLink.inertiaTensorRotation;
}
public void Start()
{
#if UNITY_2020_1_OR_NEWER
unityJoint = GetComponent <ArticulationBody>();
#else
unityJoint = GetComponent <Joint>();
#endif
}
private void DisplayJointLimits(ArticulationBody body)
{
ArticulationBody parentBody = ArticulationBodyEditorCommon.FindEnabledParentArticulationBody(body);
// Consider that the anchors are only actually matched when in play mode.
// So, if it's not play mode, we need to do that manually for the gizmos to be placed correctly.
Vector3 anchorPosition;
Quaternion anchorRotation;
if (body.matchAnchors & !EditorApplication.isPlaying)
{
anchorPosition = body.transform.TransformPoint(body.anchorPosition);
anchorRotation = body.transform.rotation * body.anchorRotation;
}
else
{
anchorPosition = parentBody.transform.TransformPoint(body.parentAnchorPosition);
anchorRotation = parentBody.transform.rotation * body.parentAnchorRotation;
}
Matrix4x4 parentAnchorSpace = Matrix4x4.TRS(anchorPosition, anchorRotation, Vector3.one);
// Show locked gizmo when root body or Fixed joint body
if (body.isRoot || body.jointType == ArticulationJointType.FixedJoint)
{
m_angularLimitHandle.xMotion = (ConfigurableJointMotion)ArticulationDofLock.LockedMotion;
m_angularLimitHandle.yMotion = (ConfigurableJointMotion)ArticulationDofLock.LockedMotion;
m_angularLimitHandle.zMotion = (ConfigurableJointMotion)ArticulationDofLock.LockedMotion;
ShowSphericalLimits(m_angularLimitHandle, body, parentAnchorSpace);
return;
}
if (body.jointType == ArticulationJointType.PrismaticJoint)
{
ShowPrismaticLimits(body, parentAnchorSpace, anchorPosition, anchorRotation);
return;
}
if (body.jointType == ArticulationJointType.RevoluteJoint)
{
// For the purposes of drawing Revolute limits, treat Z and Y motion as locked
m_angularLimitHandle.xMotion = (ConfigurableJointMotion)body.twistLock;
m_angularLimitHandle.yMotion = (ConfigurableJointMotion)ArticulationDofLock.LockedMotion;
m_angularLimitHandle.zMotion = (ConfigurableJointMotion)ArticulationDofLock.LockedMotion;
ShowRevoluteLimits(m_angularLimitHandle, body, parentAnchorSpace);
return;
}
if (body.jointType == ArticulationJointType.SphericalJoint)
{
m_angularLimitHandle.xMotion = (ConfigurableJointMotion)body.twistLock;
m_angularLimitHandle.yMotion = (ConfigurableJointMotion)body.swingYLock;
m_angularLimitHandle.zMotion = (ConfigurableJointMotion)body.swingZLock;
ShowSphericalLimits(m_angularLimitHandle, body, parentAnchorSpace);
}
}
void MoveFinger(GameObject finger, float fingerLocalOpenZ, float toLocalZ)
{
ArticulationBody articulation = finger.GetComponent <ArticulationBody>();
float targetZ = (toLocalZ - fingerLocalOpenZ) * transform.localScale.z;
var drive = articulation.zDrive;
drive.target = targetZ;
articulation.zDrive = drive;
}
private void DisplayProperAnchorHandle(ArticulationBody body, ref Vector3 anchorPos, ref Quaternion anchorRot)
{
var bodySpace = Matrix4x4.TRS(body.transform.position, body.transform.rotation, Vector3.one);
using (new Handles.DrawingScope(bodySpace))
{
anchorPos = Handles.PositionHandle(anchorPos, anchorRot);
anchorRot = Handles.RotationHandle(anchorRot, anchorPos);
}
}
public override bool AreLimitsCorrect()
{
#if UNITY_2020_1_OR_NEWER
ArticulationBody drive = GetComponent <ArticulationBody>();
return(drive.linearLockX == ArticulationDofLock.LimitedMotion && drive.xDrive.lowerLimit < drive.xDrive.upperLimit);
#else
HingeJointLimitsManager limits = GetComponent <HingeJointLimitsManager>();
return(limits != null && limits.LargeAngleLimitMin < limits.LargeAngleLimitMax);
#endif
}
public PhysicsBodySensor(ArticulationBody rootBody, PhysicsSensorSettings settings, string sensorName = null)
{
m_PoseExtractor = new ArticulationBodyPoseExtractor(rootBody);
m_SensorName = string.IsNullOrEmpty(sensorName) ? $"ArticulationBodySensor:{rootBody?.name}" : sensorName;
m_Settings = settings;
var numTransformObservations = settings.TransformSize(m_PoseExtractor.NumPoses);
m_Shape = new[] { numTransformObservations };
}
public override bool AreLimitsCorrect()
{
#if UNITY_2020_1_OR_NEWER
ArticulationBody joint = GetComponent <ArticulationBody>();
return(joint.linearLockX == ArticulationDofLock.LimitedMotion && joint.xDrive.lowerLimit < joint.xDrive.upperLimit);
#else
PrismaticJointLimitsManager limits = GetComponent <PrismaticJointLimitsManager>();
return(limits != null && limits.PositionLimitMin < limits.PositionLimitMax);
#endif
}
void Start()
{
direction = 0;
controller = (RosSharp.Control.Controller) this.GetComponentInParent(typeof(RosSharp.Control.Controller));
joint = this.GetComponent <ArticulationBody>();
controller.UpdateControlType(this);
speed = controller.speed;
torque = controller.torque;
acceleration = controller.acceleration;
}
void Start()
{
_body = GetComponent <ArticulationBody>();
_parentBody = _body.transform.parent.GetComponentInParent <ArticulationBody>();
_debugController = FindObjectOfType <MarathonTestBedController>();
_spawnableEnv = GetComponentInParent <MLAgents.SpawnableEnv>();
_mocapController = _spawnableEnv.GetComponentInChildren <MocapController>();
var mocapBodyParts = _mocapController.GetComponentsInChildren <Rigidbody>().ToList();
_target = mocapBodyParts.First(x => x.name == _body.name);
}
public void Initialize()
{
m_Body = GetComponent <ArticulationBody>();
m_DefRot = transform.rotation;
m_DefPos = transform.position;
m_DefFwd = transform.forward;
m_VelocityBuffer = new Vector3[m_BufferSize];
m_ForwardBuffer = new Vector3[m_BufferSize];
m_HeightBuffer = new float[m_BufferSize];
}
protected virtual void OnSceneGUI()
{
ArticulationBody body = (ArticulationBody)target;
if (body.isRoot)
{
return;
}
ArticulationBody parentBody = body.transform.parent.GetComponentInParent <ArticulationBody>();
{
Vector3 localAnchorT = body.anchorPosition;
Quaternion localAnchorR = body.anchorRotation;
EditorGUI.BeginChangeCheck();
DisplayProperAnchorHandle(body, ref localAnchorT, ref localAnchorR);
if (EditorGUI.EndChangeCheck())
{
Undo.RecordObject(target, "Changing Articulation body anchor position/rotation");
m_AnchorPosition.vector3Value = localAnchorT;
m_AnchorRotation.quaternionValue = localAnchorR;
body.anchorPosition = m_AnchorPosition.vector3Value;
body.anchorRotation = m_AnchorRotation.quaternionValue;
}
}
if (!m_ComputeParentAnchor.boolValue)
{
Vector3 localAnchorT = body.parentAnchorPosition;
Quaternion localAnchorR = body.parentAnchorRotation;
EditorGUI.BeginChangeCheck();
DisplayProperAnchorHandle(parentBody, ref localAnchorT, ref localAnchorR);
if (EditorGUI.EndChangeCheck())
{
Undo.RecordObject(target, "Changing Articulation body parent anchor position/rotation");
m_ParentAnchorPosition.vector3Value = localAnchorT;
m_ParentAnchorRotation.quaternionValue = localAnchorR;
body.parentAnchorPosition = m_ParentAnchorPosition.vector3Value;
body.parentAnchorRotation = m_ParentAnchorRotation.quaternionValue;
}
}
DisplayJointLimits(body);
}
protected virtual void OnSceneGUI()
{
ArticulationBody body = (ArticulationBody)target;
ArticulationBody parentBody = FindParentBody(body);
if (body.isRoot)
{
return;
}
if (!m_DisplayParentAnchor)
{
Vector3 anchorPosInWorldSpace = body.transform.TransformPoint(body.anchorPosition);
Quaternion anchorRotInWorldSpace = body.transform.rotation * body.anchorRotation;
EditorGUI.BeginChangeCheck();
DisplayProperAnchorHandle(ref anchorPosInWorldSpace, ref anchorRotInWorldSpace);
if (EditorGUI.EndChangeCheck())
{
Undo.RecordObject(target, "Changing Articulation body anchor position/rotation");
m_AnchorPosition.vector3Value = body.transform.InverseTransformPoint(anchorPosInWorldSpace);
m_AnchorRotation.quaternionValue =
Quaternion.Inverse(body.transform.rotation) * anchorRotInWorldSpace;
body.anchorPosition = m_AnchorPosition.vector3Value;
body.anchorRotation = m_AnchorRotation.quaternionValue;
}
return;
}
Vector3 parentAnchorPosInWorldSpace = body.transform.parent.TransformPoint(body.parentAnchorPosition);
Quaternion parentAnchorRotInWorldSpace = body.transform.parent.rotation * body.parentAnchorRotation;
EditorGUI.BeginChangeCheck();
DisplayProperAnchorHandle(ref parentAnchorPosInWorldSpace, ref parentAnchorRotInWorldSpace);
if (EditorGUI.EndChangeCheck())
{
Undo.RecordObject(target, "Changing Articulation body parent anchor position/rotation");
m_ParentAnchorPosition.vector3Value =
body.transform.parent.InverseTransformPoint(parentAnchorPosInWorldSpace);
m_ParentAnchorRotation.quaternionValue =
Quaternion.Inverse(body.transform.parent.rotation) * parentAnchorRotInWorldSpace;
body.parentAnchorPosition = m_ParentAnchorPosition.vector3Value;
body.parentAnchorRotation = m_ParentAnchorRotation.quaternionValue;
}
}
protected virtual void OnSceneGUI()
{
ArticulationBody body = (ArticulationBody)target;
ArticulationBody parentBody = FindParentBody(body);
if (body.isRoot)
{
return;
}
DrawFrame(body.transform.TransformPoint(body.anchorPosition), body.transform.rotation * body.anchorRotation);
DrawFrame(parentBody.transform.TransformPoint(body.parentAnchorPosition), parentBody.transform.rotation * body.parentAnchorRotation);
}
