From 762c0b701e3d0f30eca704664d13f18e61b8167e Mon Sep 17 00:00:00 2001 From: KyroVibe Date: Thu, 1 Aug 2024 11:56:33 -0600 Subject: [PATCH] Ball socket exporting, running into local to world space issues --- .../src/Parser/SynthesisParser/Joints.py | 163 +++++++++++++----- .../src/UI/ConfigCommand.py | 2 +- .../src/UI/JointConfigTab.py | 13 +- fission/src/systems/physics/PhysicsSystem.ts | 143 +++++++++++++++ 4 files changed, 277 insertions(+), 44 deletions(-) diff --git a/exporter/SynthesisFusionAddin/src/Parser/SynthesisParser/Joints.py b/exporter/SynthesisFusionAddin/src/Parser/SynthesisParser/Joints.py index c79a0240d0..e305c65fd2 100644 --- a/exporter/SynthesisFusionAddin/src/Parser/SynthesisParser/Joints.py +++ b/exporter/SynthesisFusionAddin/src/Parser/SynthesisParser/Joints.py @@ -40,6 +40,11 @@ logger = getLogger() +AcceptedJointTypes = [ + adsk.fusion.JointTypes.RevoluteJointType, + adsk.fusion.JointTypes.SliderJointType, + adsk.fusion.JointTypes.BallJointType, +] # Need to take in a graphcontainer # Need to create a new base node for each Joint Instance @@ -97,57 +102,56 @@ def populateJoints( _addRigidGroup(joint, assembly) continue - # for now if it's not a revolute or slider joint ignore it - if joint.jointMotion.jointType != 1 and joint.jointMotion.jointType != 2: - continue + if joint.jointMotion.jointType in AcceptedJointTypes: + try: + # Fusion has no instances of joints but lets roll with it anyway - try: - # Fusion has no instances of joints but lets roll with it anyway + # progressDialog.message = f"Exporting Joint configuration {joint.name}" + progressDialog.addJoint(joint.name) - # progressDialog.message = f"Exporting Joint configuration {joint.name}" - progressDialog.addJoint(joint.name) + # create the definition + joint_definition = joints.joint_definitions[joint.entityToken] + _addJoint(joint, joint_definition) - # create the definition - joint_definition = joints.joint_definitions[joint.entityToken] - _addJoint(joint, joint_definition) + # create the instance of the single definition + joint_instance = joints.joint_instances[joint.entityToken] - # create the instance of the single definition - joint_instance = joints.joint_instances[joint.entityToken] + for parse_joints in options.joints: + if parse_joints.jointToken == joint.entityToken: + guid = str(uuid.uuid4()) + signal = signals.signal_map[guid] + construct_info(joint.name, signal, GUID=guid) + signal.io = signal_pb2.IOType.OUTPUT - for parse_joints in options.joints: - if parse_joints.jointToken == joint.entityToken: - guid = str(uuid.uuid4()) - signal = signals.signal_map[guid] - construct_info(joint.name, signal, GUID=guid) - signal.io = signal_pb2.IOType.OUTPUT + # really could just map the enum to a friggin string + if parse_joints.signalType != SignalType.PASSIVE and assembly.dynamic: + if parse_joints.signalType == SignalType.CAN: + signal.device_type = signal_pb2.DeviceType.CANBUS + elif parse_joints.signalType == SignalType.PWM: + signal.device_type = signal_pb2.DeviceType.PWM - # really could just map the enum to a friggin string - if parse_joints.signalType != SignalType.PASSIVE and assembly.dynamic: - if parse_joints.signalType == SignalType.CAN: - signal.device_type = signal_pb2.DeviceType.CANBUS - elif parse_joints.signalType == SignalType.PWM: - signal.device_type = signal_pb2.DeviceType.PWM + motor = joints.motor_definitions[joint.entityToken] + fill_info(motor, joint) + simple_motor = motor.simple_motor + simple_motor.stall_torque = parse_joints.force + simple_motor.max_velocity = parse_joints.speed + simple_motor.braking_constant = 0.8 # Default for now + joint_definition.motor_reference = joint.entityToken - motor = joints.motor_definitions[joint.entityToken] - fill_info(motor, joint) - simple_motor = motor.simple_motor - simple_motor.stall_torque = parse_joints.force - simple_motor.max_velocity = parse_joints.speed - simple_motor.braking_constant = 0.8 # Default for now - joint_definition.motor_reference = joint.entityToken + joint_instance.signal_reference = signal.info.GUID + # else: + # signals.signal_map.remove(guid) - joint_instance.signal_reference = signal.info.GUID - # else: - # signals.signal_map.remove(guid) + _addJointInstance(joint, joint_instance, joint_definition, signals, options) - _addJointInstance(joint, joint_instance, joint_definition, signals, options) + # adds information for joint motion and limits + _motionFromJoint(joint.jointMotion, joint_definition) + + except: + logger.error("Failed:\n{}".format(traceback.format_exc())) + continue - # adds information for joint motion and limits - _motionFromJoint(joint.jointMotion, joint_definition) - except: - logger.error("Failed:\n{}".format(traceback.format_exc())) - continue def _addJoint(joint: adsk.fusion.Joint, joint_definition: joint_pb2.Joint): @@ -251,10 +255,10 @@ def _motionFromJoint(fusionMotionDefinition: adsk.fusion.JointMotion, proto_join 0: noop, # this should be ignored 1: fillRevoluteJointMotion, 2: fillSliderJointMotion, - 3: noop, # TODO: Implement - Ball Joint at least + 3: noop, 4: noop, # TODO: Implement 5: noop, # TODO: Implement - 6: noop, # TODO: Implement + 6: fillBallJointMotion, # TODO: Implement } fillJointMotionFunc = fillJointMotionFuncSwitcher.get(fusionMotionDefinition.jointType, lambda: None) @@ -335,6 +339,81 @@ def fillSliderJointMotion(sliderMotion: adsk.fusion.SliderJointMotion, proto_joi dof.value = sliderMotion.slideValue +def fillBallJointMotion(ballMotion: adsk.fusion.BallJointMotion, proto_joint: joint_pb2.Joint): + """#### Fill Protobuf ball joint motion data + + Args: + ballMotion (adsk.fusion.BallJointMotion): Fusion Ball Joint Data + protoJoint (joint_pb2.Joint): Protobuf joint that is being modified + """ + + # proto_joint.joint_motion_type = joint_pb2.JointMotion.REVOLUTE + proto_joint.joint_motion_type = joint_pb2.JointMotion.CUSTOM + customDofs = proto_joint.custom + + pitchDof = joint_pb2.DOF() + pitchDof.name = "pitch" + pitchDof.axis.x = ballMotion.pitchDirectionVector.x + pitchDof.axis.y = ballMotion.pitchDirectionVector.y + pitchDof.axis.z = ballMotion.pitchDirectionVector.z + if ballMotion.pitchLimits.isMaximumValueEnabled or ballMotion.pitchLimits.isMinimumValueEnabled: + pitchDof.limits.lower = ballMotion.pitchLimits.minimumValue + pitchDof.limits.upper = ballMotion.pitchLimits.maximumValue + pitchDof.value = ballMotion.pitchValue + customDofs.dofs.append(pitchDof) + + yawDof = joint_pb2.DOF() + yawDof.name = "yaw" + yawDof.axis.x = ballMotion.yawDirectionVector.x + yawDof.axis.y = ballMotion.yawDirectionVector.y + yawDof.axis.z = ballMotion.yawDirectionVector.z + if ballMotion.yawLimits.isMaximumValueEnabled or ballMotion.yawLimits.isMinimumValueEnabled: + yawDof.limits.lower = ballMotion.yawLimits.minimumValue + yawDof.limits.upper = ballMotion.yawLimits.maximumValue + yawDof.value = ballMotion.yawValue + customDofs.dofs.append(yawDof) + + rollDof = joint_pb2.DOF() + rollDof.name = "roll" + rollDof.axis.x = ballMotion.rollDirectionVector.x + rollDof.axis.y = ballMotion.rollDirectionVector.y + rollDof.axis.z = ballMotion.rollDirectionVector.z + if ballMotion.rollLimits.isMaximumValueEnabled or ballMotion.rollLimits.isMinimumValueEnabled: + rollDof.limits.lower = ballMotion.rollLimits.minimumValue + rollDof.limits.upper = ballMotion.rollLimits.maximumValue + rollDof.value = ballMotion.rollValue + customDofs.dofs.append(rollDof) + + # ballMotion. + + # dof = proto_joint.rotational.rotational_freedom + + # # name + # # axis + # # pivot + # # dynamics + # # limits + # # current value + + # dof.name = "Rotational Joint" + + # dof.value = revoluteMotion.rotationValue + + # if revoluteMotion.rotationLimits: + # dof.limits.lower = revoluteMotion.rotationLimits.minimumValue + # dof.limits.upper = revoluteMotion.rotationLimits.maximumValue + + # rotationAxisVector = revoluteMotion.rotationAxisVector + # if rotationAxisVector: + # + # else: + # rotationAxis = revoluteMotion.rotationAxis + # # don't handle 4 for now + # # There is a bug here https://jira.autodesk.com/browse/FUS-80533 + # # I have 0 memory of why this is necessary + # dof.axis.x = int(rotationAxis == 0) + # dof.axis.y = int(rotationAxis == 2) + # dof.axis.z = int(rotationAxis == 1) def noop(*argv): """Easy way to keep track of no-op code that required function pointers""" diff --git a/exporter/SynthesisFusionAddin/src/UI/ConfigCommand.py b/exporter/SynthesisFusionAddin/src/UI/ConfigCommand.py index 843fbb1d8e..d6cc361da4 100644 --- a/exporter/SynthesisFusionAddin/src/UI/ConfigCommand.py +++ b/exporter/SynthesisFusionAddin/src/UI/ConfigCommand.py @@ -143,7 +143,7 @@ def notify(self, args): *gm.app.activeDocument.design.rootComponent.allAsBuiltJoints, ]: if ( - joint.jointMotion.jointType in (JointMotions.REVOLUTE.value, JointMotions.SLIDER.value) + joint.jointMotion.jointType in (JointMotions.REVOLUTE.value, JointMotions.SLIDER.value, JointMotions.BALL.value) and not joint.isSuppressed ): jointConfigTab.addJoint(joint) diff --git a/exporter/SynthesisFusionAddin/src/UI/JointConfigTab.py b/exporter/SynthesisFusionAddin/src/UI/JointConfigTab.py index 7e34757783..919ae1ea58 100644 --- a/exporter/SynthesisFusionAddin/src/UI/JointConfigTab.py +++ b/exporter/SynthesisFusionAddin/src/UI/JointConfigTab.py @@ -14,6 +14,7 @@ createTextBoxInput, ) +from ..Parser.SynthesisParser.Joints import AcceptedJointTypes class JointConfigTab: selectedJointList: list[adsk.fusion.Joint] = [] @@ -232,6 +233,16 @@ def addJoint(self, fusionJoint: adsk.fusion.Joint, synJoint: Joint | None = None jointSpeed.tooltip = "Meters per second" self.jointConfigTable.addCommandInput(jointSpeed, row, 4) + else: + jointSpeed = commandInputs.addValueInput( + "jointSpeed", + "Speed", + "m", + adsk.core.ValueInput.createByReal(0), + ) + jointSpeed.tooltip = "Unavailable" + self.jointConfigTable.addCommandInput(jointSpeed, row, 4) + if synJoint: jointForceValue = synJoint.force * 100 # Currently a factor of 100 - Should be investigated else: @@ -486,7 +497,7 @@ def handleInputChanged( def handleSelectionEvent(self, args: adsk.core.SelectionEventArgs, selectedJoint: adsk.fusion.Joint) -> None: selectionInput = args.activeInput jointType = selectedJoint.jointMotion.jointType - if jointType == adsk.fusion.JointTypes.RevoluteJointType or jointType == adsk.fusion.JointTypes.SliderJointType: + if jointType in AcceptedJointTypes: if not self.addJoint(selectedJoint): ui = adsk.core.Application.get().userInterface result = ui.messageBox( diff --git a/fission/src/systems/physics/PhysicsSystem.ts b/fission/src/systems/physics/PhysicsSystem.ts index 42c43f213a..8a2cc60f2c 100644 --- a/fission/src/systems/physics/PhysicsSystem.ts +++ b/fission/src/systems/physics/PhysicsSystem.ts @@ -23,6 +23,7 @@ import { OnContactValidateData, PhysicsEvent, } from "./ContactEvents" +import { joltVec3ToString } from "@/util/debug/DebugPrint" export type JoltBodyIndexAndSequence = number @@ -388,6 +389,9 @@ class PhysicsSystem extends WorldSystem { case mirabuf.joint.JointMotion.SLIDER: constraints.push(this.CreateSliderConstraint(jInst, jDef, bodyA, bodyB)) break + case mirabuf.joint.JointMotion.CUSTOM: + constraints.push(this.CreateBallConstraint(jInst, jDef, bodyA, bodyB)) + break default: console.debug("Unsupported joint detected. Skipping...") break @@ -618,6 +622,145 @@ class PhysicsSystem extends WorldSystem { return [fixedConstraint, vehicleConstraint, listener] } + private CreateBallConstraint( + jointInstance: mirabuf.joint.JointInstance, + jointDefinition: mirabuf.joint.Joint, + bodyA: Jolt.Body, + bodyB: Jolt.Body + ): Jolt.Constraint { + + const sixDofConstraintSettings = new JOLT.SixDOFConstraintSettings() + + // sixDofConstraintSettings.mSwingType = JOLT.ESwingType_Pyramid + + const jointOrigin = jointDefinition.origin + ? MirabufVector3_JoltVec3(jointDefinition.origin as mirabuf.Vector3) + : new JOLT.Vec3(0, 0, 0) + // TODO: Offset transformation for robot builder. + const jointOriginOffset = jointInstance.offset + ? MirabufVector3_JoltVec3(jointInstance.offset as mirabuf.Vector3) + : new JOLT.Vec3(0, 0, 0) + + const anchorPoint = jointOrigin.Add(jointOriginOffset) + sixDofConstraintSettings.mPosition1 = sixDofConstraintSettings.mPosition2 = anchorPoint + + sixDofConstraintSettings.MakeFixedAxis(JOLT.SixDOFConstraintSettings_EAxis_TranslationX) + sixDofConstraintSettings.MakeFixedAxis(JOLT.SixDOFConstraintSettings_EAxis_TranslationY) + sixDofConstraintSettings.MakeFixedAxis(JOLT.SixDOFConstraintSettings_EAxis_TranslationZ) + + const pitchDof = jointDefinition.custom!.dofs!.at(0) + const yawDof = jointDefinition.custom!.dofs!.at(1) + const rollDof = jointDefinition.custom!.dofs!.at(2) + + const pitchAxis = new JOLT.Vec3(pitchDof?.axis?.x ?? 0, pitchDof?.axis?.y ?? 0, pitchDof?.axis?.z ?? 0) + const yawAxis = new JOLT.Vec3(yawDof?.axis?.x ?? 0, yawDof?.axis?.y ?? 0, yawDof?.axis?.z ?? 0) + const rollAxis = new JOLT.Vec3(rollDof?.axis?.x ?? 0, rollDof?.axis?.y ?? 0, rollDof?.axis?.z ?? 0) + + console.debug(`Pitch Axis: ${joltVec3ToString(pitchAxis)} ${pitchDof?.limits ? `[${pitchDof.limits.lower!.toFixed(3)}, ${pitchDof.limits.upper!.toFixed(3)}]` : ''}`) + console.debug(`Yaw Axis: ${joltVec3ToString(yawAxis)} ${yawDof?.limits ? `[${yawDof.limits.lower!.toFixed(3)}, ${yawDof.limits.upper!.toFixed(3)}]` : ''}`) + console.debug(`Roll Axis: ${joltVec3ToString(rollAxis)} ${rollDof?.limits ? `[${rollDof.limits.lower!.toFixed(3)}, ${rollDof.limits.upper!.toFixed(3)}]` : ''}`) + + const calculatedRollAxis = pitchAxis.Cross(yawAxis) + + sixDofConstraintSettings.mAxisX1 = sixDofConstraintSettings.mAxisX2 = + pitchAxis + + sixDofConstraintSettings.mAxisY1 = sixDofConstraintSettings.mAxisY2 = + yawAxis + + sixDofConstraintSettings.mMaxFriction = 0.8 + + if (pitchDof?.limits) { + if ((pitchDof.limits.upper ?? 0) - (pitchDof.limits.lower ?? 0) < 0.001) { + console.debug('Pitch Fixed') + sixDofConstraintSettings.MakeFixedAxis(JOLT.SixDOFConstraintSettings_EAxis_RotationX) + } else { + console.debug('Pitch Limited') + sixDofConstraintSettings.SetLimitedAxis(JOLT.SixDOFConstraintSettings_EAxis_RotationX, pitchDof.limits.lower ?? 0, pitchDof.limits.upper ?? 0) + } + } else { + console.debug('Pitch Free') + sixDofConstraintSettings.MakeFreeAxis(JOLT.SixDOFConstraintSettings_EAxis_RotationX) + } + + if (yawDof?.limits) { + if ((yawDof.limits.upper ?? 0) - (yawDof.limits.lower ?? 0) < 0.001) { + console.debug('Yaw Fixed') + sixDofConstraintSettings.MakeFixedAxis(JOLT.SixDOFConstraintSettings_EAxis_RotationY) + } else { + console.debug('Yaw Limited') + sixDofConstraintSettings.SetLimitedAxis(JOLT.SixDOFConstraintSettings_EAxis_RotationY, yawDof.limits.lower ?? 0, yawDof.limits.upper ?? 0) + } + } else { + console.debug('Yaw Free') + sixDofConstraintSettings.MakeFreeAxis(JOLT.SixDOFConstraintSettings_EAxis_RotationY) + } + + if (rollDof?.limits) { + if ((rollDof.limits.upper ?? 0) - (rollDof.limits.lower ?? 0) < 0.001) { + console.debug('Roll Fixed') + sixDofConstraintSettings.MakeFixedAxis(JOLT.SixDOFConstraintSettings_EAxis_RotationZ) + } else { + console.debug('Roll Limited') + sixDofConstraintSettings.SetLimitedAxis(JOLT.SixDOFConstraintSettings_EAxis_RotationZ, rollDof.limits.lower ?? 0, rollDof.limits.upper ?? 0) + } + } else { + console.debug('Roll Free') + sixDofConstraintSettings.MakeFreeAxis(JOLT.SixDOFConstraintSettings_EAxis_RotationZ) + } + + // HINGE CONSTRAINT + // const hingeConstraintSettings = new JOLT.HingeConstraintSettings() + + // const jointOrigin = jointDefinition.origin + // ? MirabufVector3_JoltVec3(jointDefinition.origin as mirabuf.Vector3) + // : new JOLT.Vec3(0, 0, 0) + // // TODO: Offset transformation for robot builder. + // const jointOriginOffset = jointInstance.offset + // ? MirabufVector3_JoltVec3(jointInstance.offset as mirabuf.Vector3) + // : new JOLT.Vec3(0, 0, 0) + + // const anchorPoint = jointOrigin.Add(jointOriginOffset) + // hingeConstraintSettings.mPoint1 = hingeConstraintSettings.mPoint2 = anchorPoint + + // const rotationalFreedom = jointDefinition.rotational!.rotationalFreedom! + + // const miraAxis = rotationalFreedom.axis! as mirabuf.Vector3 + // let axis: Jolt.Vec3 + // // No scaling, these are unit vectors + // if (versionNum < 5) { + // axis = new JOLT.Vec3(miraAxis.x ? -miraAxis.x : 0, miraAxis.y ?? 0, miraAxis.z! ?? 0) + // } else { + // axis = new JOLT.Vec3(miraAxis.x! ?? 0, miraAxis.y! ?? 0, miraAxis.z! ?? 0) + // } + // hingeConstraintSettings.mHingeAxis1 = hingeConstraintSettings.mHingeAxis2 = axis.Normalized() + // hingeConstraintSettings.mNormalAxis1 = hingeConstraintSettings.mNormalAxis2 = getPerpendicular( + // hingeConstraintSettings.mHingeAxis1 + // ) + + // // Some values that are meant to be exactly PI are perceived as being past it, causing unexpected behavior. + // // This safety check caps the values to be within [-PI, PI] wth minimal difference in precision. + // const piSafetyCheck = (v: number) => Math.min(3.14158, Math.max(-3.14158, v)) + + // if ( + // rotationalFreedom.limits && + // Math.abs((rotationalFreedom.limits.upper ?? 0) - (rotationalFreedom.limits.lower ?? 0)) > 0.001 + // ) { + // const currentPos = piSafetyCheck(rotationalFreedom.value ?? 0) + // const upper = piSafetyCheck(rotationalFreedom.limits.upper ?? 0) - currentPos + // const lower = piSafetyCheck(rotationalFreedom.limits.lower ?? 0) - currentPos + + // hingeConstraintSettings.mLimitsMin = -upper + // hingeConstraintSettings.mLimitsMax = -lower + // } + + // const constraint = hingeConstraintSettings.Create(bodyA, bodyB) + const constraint = sixDofConstraintSettings.Create(bodyA, bodyB) + this._joltPhysSystem.AddConstraint(constraint) + + return constraint + } + private IsWheel(jDef: mirabuf.joint.Joint) { return ( jDef.info!.name! != "grounded" &&