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