Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Connector: IssacSim -> Ignition #16

Merged
merged 15 commits into from
Mar 31, 2022
Merged

Connector: IssacSim -> Ignition #16

merged 15 commits into from
Mar 31, 2022

Conversation

ahcorde
Copy link
Contributor

@ahcorde ahcorde commented Feb 28, 2022

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.

@ahcorde ahcorde self-assigned this Feb 28, 2022
Copy link
Collaborator

@koonpeng koonpeng left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think ObjectsChanged should be _objectsChanged following ignition convention.

source/ignition_live/FUSDLayerNoticeListener.hpp Outdated Show resolved Hide resolved
req.mutable_position()->set_z(transforms.position[2]);

ignition::math::Quaterniond q(
transforms.rotZYX[0],
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Comment on lines 55 to 56
app.add_option("--pose", simulatorPoses, "Which simulator will handle the poses")
->required();
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@@ -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
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can this TODO be considered done with this pr?

@koonpeng
Copy link
Collaborator

koonpeng commented Mar 3, 2022

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.

@ahcorde ahcorde mentioned this pull request Mar 4, 2022
Teo Koon Peng and others added 2 commits March 16, 2022 09:40
Signed-off-by: Teo Koon Peng <[email protected]>
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
Signed-off-by: ahcorde <[email protected]>
@ahcorde
Copy link
Contributor Author

ahcorde commented Mar 16, 2022

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

ign-omni-error1

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

Signed-off-by: ahcorde <[email protected]>
@ahcorde
Copy link
Contributor Author

ahcorde commented Mar 17, 2022

I fixed the pose:
fixed_pose_ign-omni

@ahcorde
Copy link
Contributor Author

ahcorde commented Mar 17, 2022

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

panda_wrong_angle

Signed-off-by: ahcorde <[email protected]>
@ahcorde ahcorde force-pushed the ahcorde/issacsim_2_ignition branch from af1daca to 24dc155 Compare March 17, 2022 21:15
@koonpeng
Copy link
Collaborator

koonpeng commented Mar 18, 2022

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.

@ahcorde
Copy link
Contributor Author

ahcorde commented Mar 18, 2022

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
Copy link
Collaborator

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

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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 Outdated Show resolved Hide resolved
source/ignition_live/Scene.cpp Outdated Show resolved Hide resolved
source/ignition_live/Scene.cpp Outdated Show resolved Hide resolved
@@ -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)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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 Outdated Show resolved Hide resolved
source/ignition_live/main.cpp Outdated Show resolved Hide resolved
Comment on lines +356 to +359
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;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

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

FUSDLayerNoticeListener::FUSDLayerNoticeListener(
std::shared_ptr<ThreadSafe<pxr::UsdStageRefPtr>> _stage,
std::shared_ptr<ThreadSafe<pxr::UsdStageRefPtr>> &_stage,
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

@ahcorde ahcorde marked this pull request as ready for review March 31, 2022 07:19
@ahcorde ahcorde merged commit 6012a40 into main Mar 31, 2022
@ahcorde ahcorde deleted the ahcorde/issacsim_2_ignition branch March 31, 2022 07:38
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants