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