public async void MoveHereModel(bool avoid_collision = true) { List <IO.Swagger.Model.Joint> modelJoints; //joints to move the model to string robotId; if (isOrientationDetail) { try { IO.Swagger.Model.Pose pose = new IO.Swagger.Model.Pose(orientation.Orientation, DataHelper.Vector3ToPosition(TransformConvertor.UnityToROS(CurrentActionPoint.transform.position))); List <IO.Swagger.Model.Joint> startJoints = SceneManager.Instance.SelectedRobot.GetJoints(); modelJoints = await WebsocketManager.Instance.InverseKinematics(SceneManager.Instance.SelectedRobot.GetId(), SceneManager.Instance.SelectedEndEffector.GetName(), true, pose, startJoints); await PrepareRobotModel(SceneManager.Instance.SelectedRobot.GetId(), false); if (!avoid_collision) { Notifications.Instance.ShowNotification("The model is in a collision with other object!", ""); } } catch (ItemNotFoundException ex) { Notifications.Instance.ShowNotification("Unable to move here model", ex.Message); return; } catch (RequestFailedException ex) { if (avoid_collision) //if this is first call, try it again without avoiding collisions { MoveHereModel(false); } else { Notifications.Instance.ShowNotification("Unable to move here model", ex.Message); } return; } } else //joints menu { modelJoints = this.joints.Joints; robotId = this.joints.RobotId; } foreach (IO.Swagger.Model.Joint joint in modelJoints) { SceneManager.Instance.SelectedRobot.SetJointValue(joint.Name, (float)joint.Value); } }
public async void CreateActionObject() { string newActionObjectName = (string)nameInput.GetValue(); if (Base.Parameter.CheckIfAllValuesValid(actionParameters)) { List <IO.Swagger.Model.Parameter> parameters = new List <IO.Swagger.Model.Parameter>(); foreach (IParameter actionParameter in actionParameters) { if (!parametersMetadata.TryGetValue(actionParameter.GetName(), out Base.ParameterMetadata actionParameterMetadata)) { Base.Notifications.Instance.ShowNotification("Failed to create new action object", "Failed to get metadata for action object parameter: " + actionParameter.GetName()); return; } IO.Swagger.Model.ActionParameter ap = new IO.Swagger.Model.ActionParameter(name: actionParameter.GetName(), value: JsonConvert.SerializeObject(actionParameter.GetValue()), type: actionParameterMetadata.Type); parameters.Add(DataHelper.ActionParameterToParameter(ap)); } try { Ray ray = Camera.main.ScreenPointToRay(new Vector3(Screen.width / 2, Screen.height / 2, 0f)); Vector3 point = TransformConvertor.UnityToROS(GameManager.Instance.Scene.transform.InverseTransformPoint(ray.GetPoint(0.5f))); IO.Swagger.Model.Pose pose = null; if (actionObjectMetadata.HasPose) { pose = new IO.Swagger.Model.Pose(position: DataHelper.Vector3ToPosition(point), orientation: DataHelper.QuaternionToOrientation(Quaternion.identity)); } SceneManager.Instance.SelectCreatedActionObject = newActionObjectName; await Base.WebsocketManager.Instance.AddObjectToScene(newActionObjectName, actionObjectMetadata.Type, pose, parameters); callback?.Invoke(); Close(); } catch (Base.RequestFailedException e) { Base.Notifications.Instance.ShowNotification("Failed to add action", e.Message); } } }
public async void GetMarkerPosition(CameraParameters cameraParams, string image, bool autoCalibrate = false, bool force = false, bool showNotification = false) { try { // receive cameraPose from server IO.Swagger.Model.EstimatedPose markerEstimatedPose = await WebsocketManager.Instance.GetCameraPose(cameraParams, image, inverse : true); if ((float)markerEstimatedPose.Quality > anchorQuality || force) { anchorQuality = (float)markerEstimatedPose.Quality; //Notifications.Instance.ShowNotification("Marker quality", markerEstimatedPose.Quality.ToString()); IO.Swagger.Model.Pose markerPose = markerEstimatedPose.Pose; Vector3 markerPositionReceived = new Vector3((float)markerPose.Position.X, (float)markerPose.Position.Y, (float)markerPose.Position.Z); Quaternion markerRotationReceived = new Quaternion((float)markerPose.Orientation.X, (float)markerPose.Orientation.Y, (float)markerPose.Orientation.Z, (float)markerPose.Orientation.W); Matrix4x4 markerMatrix = AdjustMatrixByScreenOrientation(Matrix4x4.TRS(markerPositionReceived, markerRotationReceived, Vector3.one)); Vector3 markerPosition = TransformConvertor.OpenCVToUnity(TransformConvertor.GetPositionFromMatrix(markerMatrix)); Quaternion markerRotation = TransformConvertor.OpenCVToUnity(TransformConvertor.GetQuaternionFromMatrix(markerMatrix)); // Marker Position GameObject marker = Instantiate(MarkerPositionGameObject); marker.transform.localPosition = ARCameraTransformMatrix.MultiplyPoint3x4(markerPosition); //ARCamera.TransformPoint(markerPosition); marker.transform.localRotation = TransformConvertor.GetQuaternionFromMatrix(ARCameraTransformMatrix) * markerRotation; //ARCamera.transform.rotation * markerRotation; marker.transform.localScale = new Vector3(1f, 1f, 1f); CreateLocalAnchor(marker); //System.IO.File.WriteAllBytes(Application.persistentDataPath + "/image" + imageNum + ".jpg", m_Texture.EncodeToJPG()); //imageNum++; // Transformation Inversion to get Camera Position //markerMatrix = Matrix4x4.TRS(markerPosition, markerRotation, Vector3.one); // create translation, rotation and scaling matrix //Matrix4x4 cameraMatrix = markerMatrix.inverse; // inverse to get marker rotation matrix //cameraMatrix.SetColumn(3, Vector4.zero); // set translation column to zeros //Vector3 cameraPos = cameraMatrix.MultiplyPoint3x4(markerPosition); // transform cameraPosition by marker matrix //cameraPos = -1 * cameraPos; //if (WorldAnchorLocal != null) { // // Camera Position // GameObject camera = Instantiate(MarkerPositionGameObject, WorldAnchorLocal.transform); // camera.transform.localPosition = cameraPos; // camera.transform.localRotation = Quaternion.LookRotation(cameraMatrix.GetColumn(2), cameraMatrix.GetColumn(1)); // camera.transform.localScale = new Vector3(0.05f, 0.05f, 0.05f); //} //Notifications.Instance.ShowNotification("Marker position", "GetCameraPose inverse true"); markerDetectionState = MarkerDetectionState.Success; if (showNotification) { Notifications.Instance.ShowNotification("Calibration successful", ""); } } else { markerDetectionState = MarkerDetectionState.Failure; if (showNotification) { Notifications.Instance.ShowNotification("No markers visible", "Find some markers and try again."); } } } catch (RequestFailedException ex) { markerDetectionState = MarkerDetectionState.Failure; Debug.Log("No markers visible"); if (showNotification) { Notifications.Instance.ShowNotification("No markers visible", "Find some markers and try again."); } } }