/// <summary> /// Called upon event ColladaImporter.Instance.OnModelImported, when DAE file is imported. /// </summary> /// <param name="sender"></param> /// <param name="args">Contains imported GameObject.</param> private void OnColladaModelImported(object sender, ImportedColladaEventArgs args) { Debug.Log("URDF: Collada model imported"); Transform importedModel = args.Data.transform; UrdfRobot[] urdfRobots = importedModel.GetComponentsInParent <UrdfRobot>(true); if (urdfRobots != null) { UrdfRobot urdfRobot = urdfRobots[0]; // TODO: make sure that this robotModel check really works // check if imported model corresponds to this robot RobotModel robotModel = GetRobotModel(urdfRobot.gameObject); if (robotModel != null) { // get rid of the placeholder object (New Game Object) Transform placeholderGameObject = importedModel.parent; importedModel.SetParent(placeholderGameObject.parent, worldPositionStays: false); //TODO: Temporarily, colliders are added directly to Visuals AddColliders(importedModel.gameObject, setConvex: true); Destroy(placeholderGameObject.gameObject); robotModel.SetLinkVisualLoaded(importedModel.parent.parent.parent.name, importedModel.parent.gameObject.GetComponent <UrdfVisual>()); Debug.Log("URDF: dae model of the link: " + importedModel.parent.parent.parent.name + " imported"); } } }
public static void CorrectAxis(GameObject robot) { UrdfRobot robotScript = robot.GetComponent <UrdfRobot>(); if (robotScript.CheckOrientation()) { return; } Quaternion correctYtoZ = Quaternion.Euler(-90, 0, 90); Quaternion correctZtoY = Quaternion.Inverse((correctYtoZ)); Quaternion correction = new Quaternion(); if (robotScript.choosenAxis == ImportSettings.axisType.zAxis) { correction = correctYtoZ; } else { correction = correctZtoY; } UrdfVisual[] visualMeshList = robot.GetComponentsInChildren <UrdfVisual>(); UrdfCollision[] collisionMeshList = robot.GetComponentsInChildren <UrdfCollision>(); foreach (UrdfVisual visual in visualMeshList) { visual.transform.localRotation = visual.transform.localRotation * correction; } foreach (UrdfCollision collision in collisionMeshList) { collision.transform.localRotation = collision.transform.localRotation * correction; } robotScript.SetOrientation(); }
public static void CorrectAxis(GameObject robot, ImportSettings.axisType axis = ImportSettings.axisType.yAxis) { UrdfVisual[] visualMeshList = robot.GetComponentsInChildren <UrdfVisual>(); UrdfCollision[] collisionMeshList = robot.GetComponentsInChildren <UrdfCollision>(); UrdfRobot robotScript = robot.GetComponent <UrdfRobot>(); robotScript.choosenAxis = axis; Quaternion correctZtoY = Quaternion.Euler(-90, 0, 90); Quaternion correction = Quaternion.identity; if (axis == ImportSettings.axisType.zAxis) { correction = correctZtoY; } foreach (UrdfVisual visual in visualMeshList) { visual.transform.localRotation = correction; } foreach (UrdfCollision collision in collisionMeshList) { collision.transform.localRotation = correction; } }
/// <summary> /// Called upon event OnModelImported in UrdfAssetImporterRuntime, when 3d model is imported. /// </summary> /// <param name="sender"></param> /// <param name="args">Contains imported GameObject.</param> private void OnModelImported(object sender, ImportedModelEventArgs args) { //Debug.Log("URDF: model imported"); Transform importedModel = args.RootGameObject.transform; UrdfRobot[] urdfRobots = importedModel.GetComponentsInParent <UrdfRobot>(true); if (urdfRobots != null) { UrdfRobot urdfRobot = urdfRobots[0]; // TODO: make sure that this robotModel check really works // check if imported model corresponds to this robot RobotModel robotModel = GetRobotModel(urdfRobot.gameObject); if (robotModel != null) { if (args.CollidersOnly) { robotModel.SetLinkCollisionLoaded(importedModel.GetComponentsInParent <UrdfLink>(true)[0].name, importedModel.GetComponentsInParent <UrdfCollision>(true)[0]); } else { robotModel.SetLinkVisualLoaded(importedModel.GetComponentsInParent <UrdfLink>(true)[0].name, importedModel.GetComponentsInParent <UrdfVisual>(true)[0]); } //Debug.Log("URDF: model of the link: " + importedModel.parent.parent.parent.parent.name + " imported"); } } }
private void LoadUrdf(string fileName) { string baseDirectory = "data"; string path = Path.Combine(baseDirectory, fileName); UrdfRobot robot = UrdfLoader.FromFile(path); new UrdfToBullet(World).Convert(robot, baseDirectory); }
void syncRobotJointStates(ref UrdfRobot robot) { IntPtr cmd_handle = NativeMethods.b3RequestActualStateCommandInit(pybullet, b3RobotId); IntPtr status_handle = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd_handle); EnumSharedMemoryServerStatus status_type = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status_handle); if (status_type == EnumSharedMemoryServerStatus.CMD_ACTUAL_STATE_UPDATE_COMPLETED) { for (int i = 0; i < robot.GetComponentsInChildren <UrdfJoint>().Length; i++) { var unityJoint = robot.GetComponentsInChildren <UrdfJoint>()[i]; if (!jointNames.Contains(unityJoint.JointName)) { continue; } b3JointSensorState state = new b3JointSensorState(); NativeMethods.b3GetJointState(pybullet, status_handle, b3JointIds[i], ref state); var diff = (float)state.m_jointPosition - unityJoint.GetPosition(); if (unityJoint.JointName.Contains("lh_") || unityJoint.JointName.Contains("rh_")) { continue; //var candidates = from gi in glovesInfo // where (gi.isRight && unityJoint.JointName.Contains("rh_")) || (unityJoint.JointName.Contains("lh_") && !gi.isRight) // select gi; //var gl = candidates.FirstOrDefault(); //if (gl.jointTFs == null) continue; //// var gl = (unityJoint.JointName.Contains("rh_")) ? glovesInfo.Find(x => x.isRight) : glovesInfo.Find(x => !x.isRight); //for (int j = 0; j < gl.thumbPrevSetpoints.Count; j++) //{ // if (b3JointIds[i] == gl.handJointsIds[0][j]) // { // diff = (float)(state.m_jointPosition - gl.thumbPrevSetpoints[j]); // gl.thumbPrevSetpoints[j] = state.m_jointPosition; // //Quaternion rot = Quaternion.AngleAxis(-diff * Mathf.Rad2Deg, new Vector3(0, 0, 1));// unityJoint.UnityJoint.axis); // //unityJoint.transform.rotation = unityJoint.transform.rotation * rot; // //unityJoint.GetComponentInChildren<Rigidbody>().transform.rotation = unityJoint.transform.rotation * rot; // unityJoint.UpdateJointState(diff); // break; // } //} //continue; } unityJoint.UpdateJointState(diff); } } else { Debug.LogWarning("Cannot get robot state"); } }
private static void CreateUrdfObject() { string urdfFile = EditorUtility.OpenFilePanel( "Import local URDF", Path.Combine(Path.GetDirectoryName(Application.dataPath), "Assets"), "urdf"); if (urdfFile != "") { UrdfRobot.Create(urdfFile); } }
private static void CreateUrdfObject() { //Get path to asset, check if it's a urdf file string assetPath = AssetDatabase.GetAssetPath(Selection.activeObject); if (Path.GetExtension(assetPath)?.ToLower() == ".urdf") { UrdfRobot.Create(UrdfAssetPathHandler.GetFullAssetPath(assetPath)); } else { EditorUtility.DisplayDialog("URDF Import", "The file you selected was not a URDF file. A robot can only be imported from a valid URDF file.", "Ok"); } }
/// <summary> /// Imports URDF based on a given filename. Filename has to contain a full path. /// </summary> /// <param name="filename"></param> private void ImportUrdfObject(string filename) { UrdfRobot = UrdfRobotExtensionsRuntime.Create(filename, useUrdfMaterials: false); UrdfRobot.transform.parent = transform; UrdfRobot.transform.localPosition = Vector3.zero; UrdfRobot.transform.localEulerAngles = Vector3.zero; UrdfRobot.SetRigidbodiesIsKinematic(true); RobotModel = UrdfRobot.gameObject; LoadLinks(); Debug.Log("URDF: robot created (without models yet)"); }
public override void OnInspectorGUI() { if (buttonStyle == null) { buttonStyle = new GUIStyle(EditorStyles.miniButtonRight) { fixedWidth = 75 } } ; urdfRobot = (UrdfRobot)target; GUILayout.Space(5); GUILayout.Label("All Rigidbodies", EditorStyles.boldLabel); DisplaySettingsToggle(new GUIContent("Is Kinematic"), urdfRobot.SetRigidbodiesIsKinematic); DisplaySettingsToggle(new GUIContent("Use Gravity"), urdfRobot.SetRigidbodiesUseGravity); DisplaySettingsToggle(new GUIContent("Use Inertia from URDF", "If disabled, Unity will generate new inertia tensor values automatically."), urdfRobot.SetUseUrdfInertiaData); GUILayout.Space(5); GUILayout.Label("All Colliders", EditorStyles.boldLabel); DisplaySettingsToggle(new GUIContent("Convex"), urdfRobot.SetCollidersConvex); GUILayout.Space(5); GUILayout.Label("All Joints", EditorStyles.boldLabel); EditorGUILayout.BeginHorizontal(); EditorGUILayout.PrefixLabel("Generate Unique Joint Names"); if (GUILayout.Button("Generate", new GUIStyle(EditorStyles.miniButton) { fixedWidth = 155 })) { urdfRobot.GenerateUniqueJointNames(); } EditorGUILayout.EndHorizontal(); GUILayout.Space(5); if (GUILayout.Button("Export robot to URDF file")) { // Get existing open window or if none, make a new one: UrdfExportEditorWindow window = (UrdfExportEditorWindow)EditorWindow.GetWindow(typeof(UrdfExportEditorWindow)); window.urdfRobot = urdfRobot; window.minSize = new Vector2(500, 200); window.GetEditorPrefs(); window.Show(); } }
public RobotVisualization(UrdfRobot robot) { this.m_Robot = robot; foreach (UrdfJoint joint in robot.gameObject.GetComponentsInChildren <UrdfJoint>()) { if (!m_JointsByName.ContainsKey(joint.jointName)) { m_JointsByName.Add( joint.jointName, new JointPlacement { Joint = joint, Position = joint.transform.localPosition, Rotation = joint.transform.localRotation } ); } } }
/// <summary> /// Imports URDF based on a given filename. Filename has to contain a full path. /// <param name="filename">Filename including path to the urdf file.</param> /// <param name="robotType">Type of the robot.</param> /// </summary> private void ImportUrdfObject(string filename, string robotType) { UrdfRobot urdfRobot = UrdfRobotExtensionsRuntime.Create(filename, useColliderInVisuals: true, useUrdfMaterials: true); urdfRobot.transform.parent = transform; urdfRobot.transform.localPosition = Vector3.zero; urdfRobot.transform.localEulerAngles = Vector3.zero; urdfRobot.SetRigidbodiesIsKinematic(true); RobotModel robot = new RobotModel(robotType, urdfRobot.gameObject); robot.LoadLinks(); RobotModels[robotType].Add(robot); //Debug.Log("URDF: robot created (without models yet)"); }
public static void ExportRobotToUrdf(this UrdfRobot urdfRobot, string exportRootFolder, string exportDestination) { UrdfExportPathHandler.SetExportPath(exportRootFolder, exportDestination); urdfRobot.FilePath = Path.Combine(UrdfExportPathHandler.GetExportDestination(), urdfRobot.name + ".urdf"); Robot robot = urdfRobot.ExportRobotData(); if (robot == null) { return; } robot.WriteToUrdf(); UnityEngine.Debug.Log(robot.name + " was exported to " + UrdfExportPathHandler.GetExportDestination()); UrdfMaterial.Materials.Clear(); UrdfExportPathHandler.Clear(); AssetDatabase.Refresh(); }
private static Robot ExportRobotData(this UrdfRobot urdfRobot) { Robot robot = new Robot(); robot.ConstructFromFile(urdfRobot.FilePath, urdfRobot.gameObject.name); List <string> linkNames = new List <string>(); foreach (UrdfLink urdfLink in urdfRobot.GetComponentsInChildren <UrdfLink>()) { //Link export if (linkNames.Contains(urdfLink.name)) { EditorUtility.DisplayDialog("URDF Export Error", "URDF export failed. There are several links with the name " + urdfLink.name + ". Make sure all link names are unique before exporting this robot.", "Ok"); return(null); } robot.links.Add(urdfLink.ExportLinkData()); linkNames.Add(urdfLink.name); //Joint export UrdfJoint urdfJoint = urdfLink.gameObject.GetComponent <UrdfJoint>(); if (urdfJoint != null) { robot.joints.Add(urdfJoint.ExportJointData()); } else if (!urdfLink.IsBaseLink) { //Make sure that links with no rigidbodies are still connected to the robot by a default joint robot.joints.Add(UrdfJoint.ExportDefaultJoint(urdfLink.transform)); } } robot.materials = UrdfMaterial.Materials.Values.ToList(); robot.plugins = urdfRobot.GetComponentInChildren <UrdfPlugins>().ExportPluginsData(); return(robot); }
private void DisableVuforia() { UrdfRobot RealRobot = null; UrdfRobot GhostRobot = null; foreach (GameObject objectToClone in objectsToClone) { GameObject cloneObject = Instantiate <GameObject>(objectToClone); cloneObject.transform.parent = null; cloneObject.transform.localScale = objectToClone.transform.lossyScale; cloneObject.transform.position = objectToClone.transform.position; cloneObject.transform.rotation = objectToClone.transform.rotation; if (cloneObject.name == RealRobotName) { RealRobot = cloneObject.GetComponent <UrdfRobot>(); } else if (cloneObject.name == GhostRobotName) { GhostRobot = cloneObject.GetComponent <UrdfRobot>(); } } VuforiaBehaviour.Instance.enabled = false; if (RealRobot) { JointStatePatcher jsp = rosConnector.GetComponent <JointStatePatcher>(); jsp.UrdfRobot = RealRobot; jsp.SetSubscribeJointStates(true); } if (GhostRobot) { TrajectoryPositionsPatcher tpp = rosConnector.GetComponent <TrajectoryPositionsPatcher>(); tpp.ghostRobot = GhostRobot; tpp.SetSubscribeTrajectoryPositions(true); } }
// Start is called before the first frame update void Start() { bb = GameObject.Find("BulletBridge").GetComponent <BulletBridge>(); while (!bb.isInitialized) { bb.WaitForConnection(); } pybullet = bb.GetPhysicsServerPtr(); Debug.Log("Connected " + name + " to bullet server."); BulletBridge.MakeKinematic(GetComponentsInChildren <Rigidbody>()); resetRobotPose(); urdfRobot = GetComponent <UrdfRobot>(); var robotPath = Application.dataPath + urdfPath; b3RobotId = bb.LoadURDF(robotPath, transform.position, transform.rotation, 1); bb.AddGameObject(gameObject, b3RobotId); setupRobotJoints(); if (trackIK) { IKTargets = new List <IKTarget>() { new IKTarget(rightHandTarget, bb.b3GetLinkId("rh_palm", b3RobotId), bb.GetJointKinematicChain(b3RobotId, "rh_palm", "shoulder_right_link1")), new IKTarget(leftHandTarget, bb.b3GetLinkId("lh_palm", b3RobotId), bb.GetJointKinematicChain(b3RobotId, "lh_palm", "shoulder_left_link1")) }; } lastUpdate = Time.time * 1000; camYOffset = cameraTF.rotation.eulerAngles.y; var cmd = NativeMethods.b3InitSyncBodyInfoCommand(pybullet); var status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd); }
protected virtual void OnEnable() { urdfRobot = (UrdfRobot)serializedObject.targetObject; }
public override void OnInspectorGUI() { if (buttonStyle == null) { buttonStyle = new GUIStyle(EditorStyles.miniButtonRight) { fixedWidth = 75 } } ; urdfRobot = (UrdfRobot)target; GUILayout.Space(5); GUILayout.Label("All Rigidbodies", EditorStyles.boldLabel); DisplaySettingsToggle(new GUIContent("Use Gravity"), urdfRobot.SetRigidbodiesUseGravity); DisplaySettingsToggle(new GUIContent("Use Inertia from URDF", "If disabled, Unity will generate new inertia tensor values automatically."), urdfRobot.SetUseUrdfInertiaData); DisplaySettingsToggle(new GUIContent("Default Space"), urdfRobot.ChangeToCorrectedSpace); GUILayout.Space(5); GUILayout.Label("All Colliders", EditorStyles.boldLabel); DisplaySettingsToggle(new GUIContent("Convex"), urdfRobot.SetCollidersConvex); GUILayout.Space(5); GUILayout.Label("All Joints", EditorStyles.boldLabel); EditorGUILayout.BeginHorizontal(); EditorGUILayout.PrefixLabel("Generate Unique Joint Names"); if (GUILayout.Button("Generate", new GUIStyle(EditorStyles.miniButton) { fixedWidth = 155 })) { urdfRobot.GenerateUniqueJointNames(); } EditorGUILayout.EndHorizontal(); GUILayout.Space(5); EditorGUILayout.PropertyField(axisType, new GUIContent("Axis Type")); serializedObject.ApplyModifiedProperties(); UrdfRobotExtensions.CorrectAxis(urdfRobot.gameObject, urdfRobot.choosenAxis); GUILayout.Label("Helper Scripts", EditorStyles.boldLabel); DisplaySettingsToggle(new GUIContent("Controller Script"), urdfRobot.AddController); DisplaySettingsToggle(new GUIContent("Forward Kinematics Script"), urdfRobot.AddFkRobot); GUILayout.Space(5); if (GUILayout.Button("Export robot to URDF file")) { // Get existing open window or if none, make a new one: UrdfExportEditorWindow window = (UrdfExportEditorWindow)EditorWindow.GetWindow(typeof(UrdfExportEditorWindow)); window.urdfRobot = urdfRobot; window.minSize = new Vector2(500, 200); window.GetEditorPrefs(); window.Show(); } GUILayout.Space(5); if (GUILayout.Button("Compare URDF Files")) { CompareURDF window = (CompareURDF)EditorWindow.GetWindow(typeof(CompareURDF)); window.minSize = new Vector2(500, 200); window.GetEditorPrefs(); window.Show(); } }
public override void OnInspectorGUI() { if (buttonStyle == null) { buttonStyle = new GUIStyle(EditorStyles.miniButtonRight) { fixedWidth = 75 } } ; urdfRobot = (UrdfRobot)target; EditorGUILayout.PropertyField(axisType, new GUIContent("Axis Type")); serializedObject.ApplyModifiedProperties(); UrdfRobotExtensions.CorrectAxis(urdfRobot.gameObject); GUILayout.Space(5); GUILayout.Label("All Rigidbodies", EditorStyles.boldLabel); DisplaySettingsToggle(new GUIContent("Use Gravity", "If disabled, robot is not affected by gravity."), urdfRobot.SetRigidbodiesUseGravity, UrdfRobot.useGravity); DisplaySettingsToggle(new GUIContent("Use Inertia from URDF", "If disabled, Unity will generate new inertia tensor values automatically."), urdfRobot.SetUseUrdfInertiaData, UrdfRobot.useUrdfInertiaData); DisplaySettingsToggle(new GUIContent("Default Space"), urdfRobot.ChangeToCorrectedSpace, UrdfRobot.changetoCorrectedSpace); GUILayout.Space(5); GUILayout.Label("All Colliders", EditorStyles.boldLabel); DisplaySettingsToggle(new GUIContent("Convex"), urdfRobot.SetCollidersConvex, UrdfRobot.collidersConvex); GUILayout.Space(5); GUILayout.Label("All Joints", EditorStyles.boldLabel); EditorGUILayout.BeginHorizontal(); EditorGUILayout.PrefixLabel("Generate Unique Joint Names"); if (GUILayout.Button("Generate", new GUIStyle(EditorStyles.miniButton) { fixedWidth = 155 })) { urdfRobot.GenerateUniqueJointNames(); } EditorGUILayout.EndHorizontal(); GUILayout.Space(5); EditorGUILayout.PropertyField(axisType, new GUIContent("Axis Type", "Adjust this if the models that make up your robot are facing the wrong direction.")); serializedObject.ApplyModifiedProperties(); UrdfRobotExtensions.CorrectAxis(urdfRobot.gameObject); if (urdfRobot.GetComponent <RosSharp.Control.Controller>() == null || urdfRobot.GetComponent <RosSharp.Control.FKRobot>() == null) { GUILayout.Label("Components", EditorStyles.boldLabel); GUILayout.BeginHorizontal(); if (GUILayout.Button(urdfRobot.GetComponent <RosSharp.Control.Controller>() == null? "Add Controller": "Remove Controller")) { urdfRobot.AddController(); } if (urdfRobot.GetComponent <RosSharp.Control.FKRobot>() == null) { if (GUILayout.Button("Add Forward Kinematics")) { urdfRobot.gameObject.AddComponent <RosSharp.Control.FKRobot>(); } } GUILayout.EndHorizontal(); } GUILayout.Space(5); GUILayout.Label("URDF Files", EditorStyles.boldLabel); GUILayout.BeginHorizontal(); if (GUILayout.Button("Export robot to URDF")) { exportRoot = EditorUtility.OpenFolderPanel("Select export directory", exportRoot, ""); if (exportRoot.Length == 0) { return; } else if (!Directory.Exists(exportRoot)) { EditorUtility.DisplayDialog("Export Error", "Export root folder must be defined and folder must exist.", "Ok"); } else { urdfRobot.ExportRobotToUrdf(exportRoot); SetEditorPrefs(); } } GUILayout.Space(5); if (GUILayout.Button("Compare URDF Files")) { CompareURDF window = (CompareURDF)EditorWindow.GetWindow(typeof(CompareURDF)); window.minSize = new Vector2(500, 200); window.GetEditorPrefs(); window.Show(); } GUILayout.EndHorizontal(); }
//private UnityFibonacciActionClient UnityFibonacciActionClient; public void Awake() { UrdfRobot = GetComponents <UrdfRobot>()[0]; // gameObject.AddComponent<Status2Text>(); switch (TypeOfKSMP) { case KSMPs.ROS: //gameObject.AddComponent<RosConnector>(); //gameObject.AddComponent<PoseArrayPublisher>(); // If it does not exist if (GameObject.Find("RosConnector") == null) { RosConnectorObj = new GameObject("RosConnector"); } else { RosConnectorObj = GameObject.Find("RosConnector"); } if (!planRobot) { RosConnector = RosConnectorObj.AddComponent <RosConnector>(); gameObject.AddComponent <PoseArrayPublisher>(); } RosConnector = RosConnectorObj.GetComponent <RosConnector>(); RosConnector.RosBridgeServerUrl = NetworkMasterIP; switch (TypeOfEntity) { case EntityType.manipulator: gameObject.AddComponent <JointStatePatcher>(); JointPatcher = gameObject.GetComponent <JointStatePatcher>(); JointPatcher.UrdfRobot = UrdfRobot; JointPatcher.SetSubscribeJointStates(true); if (!planRobot) { JointStateSubscriber = gameObject.GetComponent <JointStateSubscriber>(); JointStateSubscriber.Topic = Topic; gameObject.AddComponent <ExecuteTrajFeedbackSub>(); gameObject.AddComponent <TrajectorySub>(); } else { gameObject.AddComponent <DisplayTrajectorySub>(); } /***************************** * Testing: uncomment JointState Subscri and delete DisplatT * ***************************/ //gameObject.AddComponent<DisplayTrajectorySub>(); break; case EntityType.AGV: // To be implemented break; case EntityType.drone: // To be implemented break; case EntityType.sensor: // To be implemented break; default: // To be implemented break; } break; case KSMPs.None: // To be implemented break; default: // To be implemented break; } }