Connector: IssacSim -> Ignition #16

ahcorde · 2022-02-28T17:28:37Z

Signed-off-by: ahcorde [email protected]

🎉 New feature

Summary

Communication between IssacSim -> Ignition.

Added a flag to check if the pose are set by Ignition or IssacSim

If you test this PR you might find some issue relative to this issue gazebosim/gz-sim#1358

Checklist

Signed all commits for DCO
Added tests
Added example and/or tutorial
Updated documentation (as needed)
Updated migration guide (as needed)
codecheck passed (See contributing)
All tests passed (See test coverage)
While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

Signed-off-by: ahcorde <[email protected]>

koonpeng

Some questions regarding the simulatorPoses flag.

Should it only affect the poses or should we turn off ignition->omniverse completely? From what I understand on the code, it looks like a mix currently where if ignition is the controller, the entire omni->ign code path is disabled, but when omniverse is the controller, only updating of composes from ign->omni is disabled, adding, deleting of models, updating scalings, material etc is still enabled.

Operations other than poses in theory can cause loops as well, if we add a model in ign, it will add a subtree in usd, which causes a change event, which causes a ign request to add a model and so on. Same for operations like changing a model's scale, it's material properties etc.

koonpeng · 2022-03-01T06:43:05Z

source/ignition_live/FUSDNoticeListener.hpp

-    // {
-    //   ignmsg << "Changed Info Path: " << Path.GetText() << std::endl;
-    // }
+    for (const pxr::SdfPath &path : ObjectsChanged.GetResyncedPaths())


I think ObjectsChanged should be _objectsChanged following ignition convention.

source/ignition_live/FUSDLayerNoticeListener.hpp

koonpeng · 2022-03-01T06:57:39Z

source/ignition_live/FUSDNoticeListener.hpp

+        req.mutable_position()->set_z(transforms.position[2]);
+
+        ignition::math::Quaterniond q(
+          transforms.rotZYX[0],


Shouldn't it be rotXYZ instead? I realized this is just naming and GetOp indeed uses pxr::UsdGeomXformOp::TypeRotateXYZ.

koonpeng · 2022-03-01T07:00:22Z

source/ignition_live/main.cpp

+  app.add_option("--pose", simulatorPoses, "Which simulator will handle the poses")
+      ->required();


Not sure on the CLI11 feature set, but if it supports it, we should use enums.

koonpeng · 2022-03-01T07:04:17Z

source/ignition_live/Scene.cpp

-                                  ignition::math::Angle(quat.Pitch()).Degree(),
-                                  ignition::math::Angle(quat.Yaw()).Degree()),
-                     pxr::UsdGeomXformCommonAPI::RotationOrderXYZ);
+  if (this->ignitionControlPoses)


Would it be better to turn off subscription to the scene, joints and poses instead?

koonpeng · 2022-03-01T07:05:20Z

source/ignition_live/Scene.cpp

@@ -885,6 +904,28 @@ bool Scene::Init()
    ignmsg << "Subscribed to topic: [" << topic << "]" << std::endl;
  }

+  if (!this->dataPtr->ignitionControlPoses)
+  {
+    // TODO: disabled omniverse -> ignition sync to focus on ignition -> omniverse


Can this TODO be considered done with this pr?

…nition

Signed-off-by: ahcorde <[email protected]>

koonpeng · 2022-03-03T02:58:50Z

I'm getting service call timeout on the connector

[Dbg] [FUSDNoticeListener.hpp:234] path /shapes/box/box_link/box_visual/geometry.xformOp:translate
[Err] [FUSDNoticeListener.hpp:281] Service call timed out
[Dbg] [FUSDNoticeListener.hpp:234] path /shapes/box/box_link/box_visual/geometry.xformOp:translate
[Err] [FUSDNoticeListener.hpp:281] Service call timed out
[Dbg] [FUSDNoticeListener.hpp:234] path /shapes/box/box_link/box_visual/geometry.xformOp:translate

There are also errors on ignition

[Err] [UserCommands.cc:1337] Unable to update the pose for entity id:[0], name[geometry]
[Err] [UserCommands.cc:1337] Unable to update the pose for entity id:[0], name[geometry]
[Err] [UserCommands.cc:1337] Unable to update the pose for entity id:[0], name[geometry]
[Err] [UserCommands.cc:1337] Unable to update the pose for entity id:[0], name[geometry]

I think the problem is that usd allows transformations on geometry but not sdf, so when trying to translate that to sdf we get a non existing target.

I notice ignition physics is still running when isaacsim updates a pose, in theory this could lead to conflicting states. I can't test this now because I had to re-install omniverse to fix some other issues and downloads are down so I can't re-install isaacsim, and because of the above errors.

Signed-off-by: Teo Koon Peng <[email protected]> Co-authored-by: Alejandro Hernández Cordero <[email protected]>

Signed-off-by: ahcorde <[email protected]>

ahcorde · 2022-03-16T22:11:46Z

@koonpeng I was testing your changes, and I'm facing this issue. Translations look wrong

You need this change in ign-gazebo gazebosim/gz-sim#1394

Signed-off-by: ahcorde <[email protected]>

ahcorde · 2022-03-17T21:01:25Z

I fixed the pose:

ahcorde · 2022-03-17T21:05:54Z

But we are facing another issue. Issac Sim dos not provide the joint angle, we can only see changes in the links

Require: gazebosim/gz-sim#1394

I tried to implement something to calculate the joint angle but it's wrong af1daca

Signed-off-by: ahcorde <[email protected]>

koonpeng · 2022-03-18T09:05:28Z

But we are facing another issue. Issac Sim dos not provide the joint angle, we can only see changes in the links

Require: ignitionrobotics/ign-gazebo#1394

I tried to implement something to calculate the joint angle but it's wrong af1daca

Wasn't able to try this yet. I'm assuming this is the simulation being ran? https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/tutorial_ros_moveit.html

~~Tried to follow the tutorial but getting 404 errors when installing some ros packages.~~ Unfortunately I cannot run the sim with my current setup as it is now, it requires both ros2 and omniverse. My ros2 environment is in a container, trying to install nucleus in the container doesn't work for some unknown reason. Either I try to make omniverse work in the container or build ros + ignition on my host.

Signed-off-by: ahcorde <[email protected]>

ahcorde · 2022-03-18T22:05:56Z

to test this you can run this script using ROS 1:

You should add the following ROS plugins: Clock, Tree pose and Joint state

#!/usr/bin/env python
# Copyright (c) 2020-2021, NVIDIA CORPORATION.  All rights reserved.
#
# NVIDIA CORPORATION and its licensors retain all intellectual property
# and proprietary rights in and to this software, related documentation
# and any modifications thereto.  Any use, reproduction, disclosure or
# distribution of this software and related documentation without an express
# license agreement from NVIDIA CORPORATION is strictly prohibited.

import rospy
from sensor_msgs.msg import JointState
import numpy as np
import time

rospy.init_node("test_rosbridge", anonymous=True)

pub = rospy.Publisher("/joint_command", JointState, queue_size=10)
joint_state = JointState()

joint_state.name = [
  "panda_joint1",
  "panda_joint2",
  "panda_joint3",
  "panda_joint4",
  "panda_joint5",
  "panda_joint6",
  "panda_joint7",
  "panda_finger_joint1",
  "panda_finger_joint2"
]

num_joints = len(joint_state.name)


# make sure kit's editor is playing for receiving messages ##

joint_state.position = np.array([0.0] * num_joints)
default_joints = [0.0, 1.57, 0.707, 0.707, 0, 3.1416, 0, 0, 0]

# limiting the movements to a smaller range (this is not the range of the robot, just the range of the movement
max_joints = np.array(default_joints) + 0.707
min_joints = np.array(default_joints) - 0.07

# position control the robot to wiggle around each joint
time_start = time.time()
rate = rospy.Rate(20)
while not rospy.is_shutdown():
    joint_state.position = [45.0*3.1416/180.0,
                            15.0*3.1416/180.0,
                            35.0*3.1416/180.0,
                            80.0*3.1416/180.0,
                            10.0*3.1416/180.0,
                            30.0*3.1416/180.0,
                            20.0*3.1416/180.0, 0, 0]
    joint_state.position = np.sin(time.time() - time_start) * (max_joints - min_joints) * 0.5 + default_joints
    #joint_state.position = default_joints
    pub.publish(joint_state)
    rate.sleep()

koonpeng · 2022-03-22T07:23:45Z

managed to run the sim in isaac but wasn't able to get the connector to work

2022-03-22.15-20-02.mp4

ignition logs

<same>
[Err] [UserCommands.cc:761] Unable to update the pose for entity id:[0], name[panda_hand]
[Err] [UserCommands.cc:761] Unable to update the pose for entity id:[0], name[panda_hand]
[Err] [UserCommands.cc:761] Unable to update the pose for entity id:[0], name[panda_hand]
[Err] [UserCommands.cc:761] Unable to update the pose for entity id:[0], name[panda_hand]
[Err] [UserCommands.cc:761] Unable to update the pose for entity id:[0], name[panda_hand]
[Err] [UserCommands.cc:761] Unable to update the pose for entity id:[0], name[panda_hand]

ign-omni logs

<same>
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link7.xformOp:translate
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link8.physics:angularVelocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link8.physics:velocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link8.xformOp:orient
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link8.xformOp:translate
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_rightfinger.physics:angularVelocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_rightfinger.physics:velocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_rightfinger.xformOp:orient
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_rightfinger.xformOp:translate
[Err] [FUSDNoticeListener.hpp:450] Service call timed out
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_hand.physics:angularVelocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_hand.physics:velocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_hand.xformOp:orient
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_hand.xformOp:translate
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_leftfinger.physics:angularVelocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_leftfinger.physics:velocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_leftfinger.xformOp:orient
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_leftfinger.xformOp:translate
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link1.physics:angularVelocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link1.xformOp:orient
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link1.xformOp:translate
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link2.physics:angularVelocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link2.xformOp:orient
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link2.xformOp:translate
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link3.physics:angularVelocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link3.physics:velocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link3.xformOp:orient
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link3.xformOp:translate
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link4.physics:angularVelocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link4.physics:velocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link4.xformOp:orient
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link4.xformOp:translate
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link5.physics:angularVelocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link5.physics:velocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link5.xformOp:orient
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link5.xformOp:translate
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link6.physics:angularVelocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link6.physics:velocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link6.xformOp:orient
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link6.xformOp:translate
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link7.physics:angularVelocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link7.physics:velocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link7.xformOp:orient
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link7.xformOp:translate
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link8.physics:angularVelocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link8.physics:velocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link8.xformOp:orient
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_link8.xformOp:translate
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_rightfinger.physics:angularVelocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_rightfinger.physics:velocity
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_rightfinger.xformOp:orient
[Dbg] [FUSDNoticeListener.hpp:326] path /panda/panda_rightfinger.xformOp:translate

Signed-off-by: ahcorde <[email protected]>

koonpeng · 2022-03-28T04:15:25Z

source/ignition_live/Scene.cpp

+  std::size_t found = visualName.find("_visual");
+  if (found != std::string::npos)
+  {
+    if (visualName.substr(found).find("_visual") !=


This condition will always be true since found != std::string::npos is true, a substr of the found index will always contain the search pattern.

koonpeng · 2022-03-28T04:17:32Z

source/ignition_live/Scene.cpp

+  std::size_t found = linkName.find("_link");
+  if (found != std::string::npos)
+  {
+    if (linkName.substr(found).find("_link") !=


This condition will always be true since found != std::string::npos is true, a substr of the found index will always contain the search pattern.

source/ignition_live/Scene.cpp

koonpeng · 2022-03-28T04:27:49Z

source/ignition_live/Scene.cpp

@@ -578,6 +628,96 @@ bool Scene::Implementation::UpdateModel(const ignition::msgs::Model &_model)
  auto stage = this->stage->Lock();

  std::string modelName = _model.name();
+
+  auto range = pxr::UsdPrimRange::Stage(*stage);
+  for (auto const &prim : range)


Traversing the stage for each update might be bad for performance, maybe add some comments explaining a situation that will cause a model to be in multiple prims and possible improvements to avoid that.

source/ignition_live/Scene.cpp

source/ignition_live/main.cpp

koonpeng · 2022-03-28T04:38:19Z

source/ignition_live/FUSDNoticeListener.cpp

+          qX = ignition::math::Quaterniond(angleX.Normalized().Radian(), 0, 0);
+          qY = ignition::math::Quaterniond(0, angleY.Normalized().Radian(), 0);
+          qZ = ignition::math::Quaterniond(0, 0, angleZ.Normalized().Radian());
+          q = ((q * qX) * qY) * qZ;


Since this is rotXYZ, we should be able to use the rpy constructor.

koonpeng · 2022-03-28T04:52:23Z

source/ignition_live/FUSDNoticeListener.cpp

+        continue;
+      auto stage = this->dataPtr->stage->Lock();
+      igndbg << "path " << objectsChanged.GetText() << std::endl;
+      auto modelUSD = stage->GetPrimAtPath(objectsChanged.GetParentPath());


Wasn't able to test this as I can't get the panda sim working. My only guess for joints orientation is that maybe the pose is being applied twice, first when calculating the joints position and then again here when calculating the pose?

Signed-off-by: ahcorde <[email protected]>

…nition

Signed-off-by: ahcorde <[email protected]>

koonpeng · 2022-03-31T06:25:37Z

source/ignition_live/FUSDLayerNoticeListener.cpp

@@ -35,7 +35,7 @@ class FUSDLayerNoticeListener::Implementation
 };

 FUSDLayerNoticeListener::FUSDLayerNoticeListener(
-  std::shared_ptr<ThreadSafe<pxr::UsdStageRefPtr>> _stage,
+  std::shared_ptr<ThreadSafe<pxr::UsdStageRefPtr>> &_stage,


I guess this fixed the double lock issue? Not sure why that happens and how this fixes it, I guess the mutex got copied for some reason?

Connector: IssacSim -> Ignition

c90eea6

Signed-off-by: ahcorde <[email protected]>

ahcorde requested review from adlarkin and koonpeng February 28, 2022 17:28

ahcorde self-assigned this Feb 28, 2022

koonpeng reviewed Mar 1, 2022

View reviewed changes

ahcorde added 2 commits March 1, 2022 15:13

Merge remote-tracking branch 'origin/main' into ahcorde/issacsim_2_ig…

c5d1fc1

…nition

Added feedback

ffdd1f0

Signed-off-by: ahcorde <[email protected]>

ahcorde mentioned this pull request Mar 4, 2022

SDF <-> USD connector #18

Open

Teo Koon Peng and others added 2 commits March 16, 2022 09:40

Workaround model sync (#19)

f858a4d

Signed-off-by: Teo Koon Peng <[email protected]> Co-authored-by: Alejandro Hernández Cordero <[email protected]>

Added some fixes

7edfc7d

Signed-off-by: ahcorde <[email protected]>

Some fixes

91f76eb

Signed-off-by: ahcorde <[email protected]>

Joints

24dc155

Signed-off-by: ahcorde <[email protected]>

ahcorde force-pushed the ahcorde/issacsim_2_ignition branch from af1daca to 24dc155 Compare March 17, 2022 21:15

improve joint and whem we should add a link or visual

e7bc26c

Signed-off-by: ahcorde <[email protected]>

ahcorde added 2 commits March 24, 2022 16:35

include some improvements

d572087

Signed-off-by: ahcorde <[email protected]>

Improve implemenetation

b23402e

Signed-off-by: ahcorde <[email protected]>

koonpeng requested changes Mar 28, 2022

View reviewed changes

ahcorde added 5 commits March 28, 2022 14:02

Added feedback

5ac2ab0

Signed-off-by: ahcorde <[email protected]>

Merge remote-tracking branch 'origin/main' into ahcorde/issacsim_2_ig…

78e43db

…nition

Fix joint issue

c9f346a

Signed-off-by: ahcorde <[email protected]>

Restructure tutorials

4d6daa0

Signed-off-by: ahcorde <[email protected]>

Fix some issues

e952b99

Signed-off-by: ahcorde <[email protected]>

koonpeng approved these changes Mar 31, 2022

View reviewed changes

ahcorde marked this pull request as ready for review March 31, 2022 07:19

ahcorde merged commit 6012a40 into main Mar 31, 2022

ahcorde deleted the ahcorde/issacsim_2_ignition branch March 31, 2022 07:38

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Connector: IssacSim -> Ignition #16

Connector: IssacSim -> Ignition #16

ahcorde commented Feb 28, 2022

koonpeng left a comment

koonpeng Mar 1, 2022

koonpeng Mar 1, 2022

koonpeng Mar 1, 2022

koonpeng Mar 1, 2022

koonpeng Mar 1, 2022

koonpeng commented Mar 3, 2022 •

edited

Loading

ahcorde commented Mar 16, 2022

ahcorde commented Mar 17, 2022

ahcorde commented Mar 17, 2022

koonpeng commented Mar 18, 2022 •

edited

Loading

ahcorde commented Mar 18, 2022

koonpeng commented Mar 22, 2022

koonpeng Mar 28, 2022

koonpeng Mar 28, 2022

koonpeng Mar 28, 2022

koonpeng Mar 28, 2022

koonpeng Mar 28, 2022

koonpeng Mar 31, 2022

		app.add_option("--pose", simulatorPoses, "Which simulator will handle the poses")
		->required();

Connector: IssacSim -> Ignition #16

Connector: IssacSim -> Ignition #16

Conversation

ahcorde commented Feb 28, 2022

🎉 New feature

Summary

Checklist

koonpeng left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

koonpeng commented Mar 3, 2022 • edited Loading

ahcorde commented Mar 16, 2022

ahcorde commented Mar 17, 2022

ahcorde commented Mar 17, 2022

koonpeng commented Mar 18, 2022 • edited Loading

ahcorde commented Mar 18, 2022

koonpeng commented Mar 22, 2022

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

koonpeng commented Mar 3, 2022 •

edited

Loading

koonpeng commented Mar 18, 2022 •

edited

Loading