From a46370b8040cf8e8e512075b587c677a31d482f3 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Fri, 18 Dec 2020 12:49:05 -0800 Subject: [PATCH 01/35] Merging dev branch into main (#78) * Reverting params.yaml and updating code in server_endpoint.md * TcpServer class was renamed * convert all files with dos2unix (#76) * Minor tutorial tweaks, update version numbers Co-authored-by: Devin Miller Co-authored-by: Peifeng Jing --- .gitignore | 3 +- .gitmodules | 3 - .../Packages/manifest.json | 5 +- .../niryo_moveit/scripts/server_endpoint.py | 19 ++-- .../pick_and_place/ROS/src/ros_tcp_endpoint | 1 - .../robotics_demo/scripts/server_endpoint.py | 21 ++-- tutorials/ros_unity_integration/publisher.md | 45 +++----- .../ros_unity_integration/server_endpoint.md | 101 ++++++++---------- 8 files changed, 87 insertions(+), 111 deletions(-) delete mode 160000 tutorials/pick_and_place/ROS/src/ros_tcp_endpoint diff --git a/.gitignore b/.gitignore index 0653b96a..0382a3e3 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,4 @@ .DS_Store -.swp .idea .vscode/ -tutorials/pick_and_place/PickAndPlaceProject/Packages/packages-lock.json +tutorials/pick_and_place/PickAndPlaceProject/Packages/packages-lock.json diff --git a/.gitmodules b/.gitmodules index 015a49a4..abd504f1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,6 +4,3 @@ [submodule "tutorials/pick_and_place/ROS/src/moveit_msgs"] path = tutorials/pick_and_place/ROS/src/moveit_msgs url = https://github.com/ros-planning/moveit_msgs.git -[submodule "tutorials/pick_and_place/ROS/src/ros_tcp_endpoint"] - path = tutorials/pick_and_place/ROS/src/ros_tcp_endpoint - url = https://github.com/Unity-Technologies/ROS-TCP-Endpoint.git diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json index c0a03b65..20a48000 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json +++ b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json @@ -4,9 +4,8 @@ "com.unity.ide.rider": "2.0.7", "com.unity.ide.visualstudio": "2.0.3", "com.unity.ide.vscode": "1.2.2", - "com.unity.robotics.ros-tcp-connector": - "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#v0.3.0", - "com.unity.robotics.urdf-importer": "https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.3.0", + "com.unity.robotics.urdf-importer": "https://github.com/Unity-Technologies/URDF-Importer.git#v0.0.4", + "com.unity.robotics.ros-tcp-connector": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git#v0.1.1", "com.unity.test-framework": "1.1.18", "com.unity.textmeshpro": "3.0.1", "com.unity.timeline": "1.4.3", diff --git a/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py b/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py index 3dc74ebe..53674b09 100755 --- a/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py +++ b/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py @@ -3,24 +3,25 @@ import rospy from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService + from niryo_moveit.msg import NiryoMoveitJoints, NiryoTrajectory -from niryo_moveit.srv import MoverService, MoverServiceRequest +from niryo_moveit.srv import MoverService -from niryo_one_msgs.msg import RobotMoveActionGoal def main(): ros_node_name = rospy.get_param("/TCP_NODE_NAME", 'TCPServer') tcp_server = TcpServer(ros_node_name) - rospy.init_node(ros_node_name, anonymous=True) - # Start the Server Endpoint with a ROS communication objects dictionary for routing messages - tcp_server.start({ + # Create ROS communication objects dictionary for routing messages + tcp_server.source_destination_dict = { 'SourceDestination_input': RosPublisher('SourceDestination', NiryoMoveitJoints, queue_size=10), 'NiryoTrajectory': RosSubscriber('NiryoTrajectory', NiryoTrajectory, tcp_server), - 'niryo_moveit': RosService('niryo_moveit', MoverService), - 'niryo_one/commander/robot_action/goal': RosSubscriber('niryo_one/commander/robot_action/goal', RobotMoveActionGoal, tcp_server), - 'sim_real_pnp': RosPublisher('sim_real_pnp', MoverServiceRequest) - }) + 'niryo_moveit': RosService('niryo_moveit', MoverService) + } + + # Start the Server Endpoint + rospy.init_node(ros_node_name, anonymous=True) + tcp_server.start() rospy.spin() diff --git a/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint b/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint deleted file mode 160000 index a5e0fd1c..00000000 --- a/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint +++ /dev/null @@ -1 +0,0 @@ -Subproject commit a5e0fd1ce1a711f557fe67e73657c74f92482fbe diff --git a/tutorials/ros_packages/robotics_demo/scripts/server_endpoint.py b/tutorials/ros_packages/robotics_demo/scripts/server_endpoint.py index ababf68c..0485d971 100644 --- a/tutorials/ros_packages/robotics_demo/scripts/server_endpoint.py +++ b/tutorials/ros_packages/robotics_demo/scripts/server_endpoint.py @@ -2,24 +2,25 @@ import rospy -from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService, UnityService +from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService + from robotics_demo.msg import PosRot, UnityColor -from robotics_demo.srv import PositionService, ObjectPoseService +from robotics_demo.srv import PositionService def main(): ros_node_name = rospy.get_param("/TCP_NODE_NAME", 'TCPServer') buffer_size = rospy.get_param("/TCP_BUFFER_SIZE", 1024) connections = rospy.get_param("/TCP_CONNECTIONS", 10) tcp_server = TcpServer(ros_node_name, buffer_size, connections) - rospy.init_node(ros_node_name, anonymous=True) - - tcp_server.start({ + + tcp_server.source_destination_dict = { + 'pos_srv': RosService('position_service', PositionService), 'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10), - 'color': RosSubscriber('color', UnityColor, tcp_server), - 'pos_srv': RosService('pos_srv', PositionService), - 'obj_pose_srv': UnityService('obj_pose_srv', ObjectPoseService, tcp_server), - }) - + 'color': RosSubscriber('color', UnityColor, tcp_server) + } + + rospy.init_node(ros_node_name, anonymous=True) + tcp_server.start() rospy.spin() diff --git a/tutorials/ros_unity_integration/publisher.md b/tutorials/ros_unity_integration/publisher.md index 502d1ab2..8ebbf0a6 100644 --- a/tutorials/ros_unity_integration/publisher.md +++ b/tutorials/ros_unity_integration/publisher.md @@ -9,10 +9,10 @@ Create a simple Unity scene which publishes a GameObject's position and rotation - Follow the [ROS–Unity Initial Setup](setup.md) guide. - Open a new terminal window and run the following commands: - + ```bash source devel/setup.bash - rosrun robotics_demo server_endpoint.py + rosrun robotics_demo server_endpoint.py ``` Once the server_endpoint has started, it will print something similar to `[INFO] [1603488341.950794]: Starting server on 192.168.50.149:10000`. @@ -20,36 +20,32 @@ Once the server_endpoint has started, it will print something similar to `[INFO] - Open another new terminal window, navigate to your ROS workspace, and run the following commands: ```bash source devel/setup.bash - rostopic echo pos_rot + rostopic echo pos_rot ``` ## Setting Up Unity Scene -- In the menu bar, find and select `Robotics` -> `Generate ROS Messages...` -- Set the ROS message path to `PATH/TO/Unity-Robotics-Hub/tutorials/ros_packages/robotics_demo`. - - Expand the robotics_demo subfolder and click "Build 2 msgs" to generate new C# scripts from the ROS .msg files. - -![](images/generate_messages_1.png) - - - The generated files will be saved in the default directory `Assets/RosMessages/RoboticsDemo/msg`. +- In the menu bar, find and select `RosMessageGeneration` -> `Auto Generate Messages` -> `Single Message ...` +- Set the input file path to `PATH/TO/Unity-Robotics-Hub/tutorials/ros_packages/robotics_demo/msg/PosRot.msg ` and click `GENERATE!` + - The generated file will be saved in the default directory `Assets/RosMessages/msg` - Create a new directory in `Assets` and name it `Scripts` - Create a new script in the `Scripts` directory and name it `RosPublisherExample.cs` - Open `RosPublisherExample.cs` and paste the following code: - - **Note** Script can be found at `tutorials/ros_unity_integration/unity_scripts` + - **Note** Script can be found at `tutorials/ros_unity_integration/unity_scripts` ```csharp using RosMessageTypes.RoboticsDemo; using UnityEngine; -using Unity.Robotics.ROSTCPConnector; +using Random = UnityEngine.Random; /// -/// +/// /// public class RosPublisherExample : MonoBehaviour { - ROSConnection ros; + public ROSConnection ros; public string topicName = "pos_rot"; - // The game object + // The GameObject public GameObject cube; // Publish the cube's position and rotation every N seconds public float publishMessageFrequency = 0.5f; @@ -57,12 +53,6 @@ public class RosPublisherExample : MonoBehaviour // Used to determine how much time has elapsed since the last message was published private float timeElapsed; - void Start() - { - // start the ROS connection - ros = ROSConnection.instance; - } - private void Update() { timeElapsed += Time.deltaTime; @@ -71,7 +61,7 @@ public class RosPublisherExample : MonoBehaviour { cube.transform.rotation = Random.rotation; - MPosRot cubePos = new MPosRot( + PosRot cubePos = new PosRot( cube.transform.position.x, cube.transform.position.y, cube.transform.position.z, @@ -92,15 +82,14 @@ public class RosPublisherExample : MonoBehaviour - Add a plane and a cube to the empty Unity scene - Move the cube a little ways up so it is hovering above the plane -- In the main menu bar, open `Robotics/ROS Settings`. - - Set the ROS IP address and port to match the ROS IP and port variables defined when you set up ROS. +- Create an empty GameObject, name it `RosConnection` and attach the `ROS TCP Connection/Runtime/TcpConnector/ROSConnection` script. + - Change the host name and port to match the ROS IP and port variables defined when you set up ROS - Create another empty GameObject, name it `RosPublisher` and attach the `RosPublisherExample` script. - - Drag the cube GameObject onto the `Cube` parameter. + - Drag the cube GameObject onto the `Cube` parameter + - Drag the RosConnection object onto its `Ros` parameter. - Pressing play in the Editor should publish a message to the terminal running the `rostopic echo pos_rot` command every 0.5 seconds -> Please reference [networking troubleshooting](network.md) doc if any errors are thrown. - ![](images/tcp_1.gif) -Continue to the [ROS Subscriber](subscriber.md) tutorial. \ No newline at end of file +Continue to the [ROS Subscriber](subscriber.md) tutorial. diff --git a/tutorials/ros_unity_integration/server_endpoint.md b/tutorials/ros_unity_integration/server_endpoint.md index a27fc7e6..d75f1ded 100644 --- a/tutorials/ros_unity_integration/server_endpoint.md +++ b/tutorials/ros_unity_integration/server_endpoint.md @@ -5,7 +5,7 @@ A walkthrough of the important components of a ROS TCP endpoint script using the The following is an example of a server endpoint Python script that: - Gets parameters from `rosparam` -- Creates corresponding ROS Publisher, Subscriber, and Service objects to interact with topics and services running in ROS network +- Creates corresponding ROS Publisher, Subscriber, and Service objects to interact with topics and services running in in ROS network - Starts TCP Server process to handle incoming and outgoing connections @@ -14,24 +14,25 @@ The following is an example of a server endpoint Python script that: import rospy -from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService, UnityService +from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService + from robotics_demo.msg import PosRot, UnityColor -from robotics_demo.srv import PositionService, ObjectPoseService +from robotics_demo.srv import PositionService def main(): ros_node_name = rospy.get_param("/TCP_NODE_NAME", 'TCPServer') buffer_size = rospy.get_param("/TCP_BUFFER_SIZE", 1024) connections = rospy.get_param("/TCP_CONNECTIONS", 10) tcp_server = TcpServer(ros_node_name, buffer_size, connections) - rospy.init_node(ros_node_name, anonymous=True) - tcp_server.start({ + tcp_server.source_destination_dict = { + 'pos_srv': RosService('position_service', PositionService), 'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10), - 'color': RosSubscriber('color', UnityColor, tcp_server), - 'pos_srv': RosService('pos_srv', PositionService), - 'obj_pose_srv': UnityService('obj_pose_srv', ObjectPoseService, tcp_server), - }) + 'color': RosSubscriber('color', UnityColor, tcp_server) + } + rospy.init_node(ros_node_name, anonymous=True) + tcp_server.start() rospy.spin() @@ -42,22 +43,12 @@ if __name__ == "__main__": ## Import Statements for Services and Messages ```python -from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService, UnityService -from robotics_demo.msg import PosRot, UnityColor -from robotics_demo.srv import PositionService, ObjectPoseService -``` - -## Creating the Server - -Requires: +from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService -- The ROS node name - -```python - tcp_server = TcpServer(ros_node_name, buffer_size, connections) +from robotics_demo.msg import PosRot, UnityColor +from robotics_demo.srv import PositionService ``` -The `ros_node_name` argument is required and the `buffer_size` and `connections` are optional. They are set to `1024` and `10` by default if not provided in the constructor arguments. ## Instantiate the ROS Node @@ -65,34 +56,16 @@ The `ros_node_name` argument is required and the `buffer_size` and `connections` rospy.init_node(ros_node_name, anonymous=True) ``` -## Starting the Server - -```python - tcp_server.start({ - 'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10), - 'color': RosSubscriber('color', UnityColor, tcp_server), - 'pos_srv': RosService('pos_srv', PositionService), - 'obj_pose_srv': UnityService('obj_pose_srv', ObjectPoseService, tcp_server), - }) - - rospy.spin() -``` - -## Source Destination Dictionary - -The argument to start() is a dictionary keyed by topic or service with the corresponding ROS communication class as the value. The dictionary is used by the TCP server to direct messages to and from the ROS network. - ## ROS Publisher -A ROS Publisher allows a Unity component to send messages on a given topic to other ROS nodes. It requires three components: +A ROS Publisher requires three components: - Topic name - ROS message class generated from running `catkin_make` command -- Queue size (optional) +- Queue size `RosPublisher('pos_rot', PosRot, queue_size=10)` - ## ROS Subscriber -A ROS Subscriber allows a Unity component to receive messages from other ROS nodes on a given topic. It requires three components: +A ROS Subscriber requires three components: - Topic name - ROS message class generated from running `catkin_make` command @@ -101,25 +74,43 @@ A ROS Subscriber allows a Unity component to receive messages from other ROS nod `RosSubscriber('color', UnityColor, tcp_server)` ## ROS Service -A ROS Service is similar to a RosPublisher, in that a Unity component sends a Request message to another ROS node. Unlike a Publisher, the Unity component then waits for a Response back. It requires two components: +A ROS Service requires two components: - Service name - ROS Service class generated from running `catkin_make` command -`RosService('pos_srv', PositionService)` +`RosService('position_service', PositionService)` -## Unity Service +## Creating the Server -A Unity Service is similar to a RosSubscriber, in that a Unity component receives a Request message from another ROS node. It then sends a Response back. It requires three components: +Requires: -- Service name -- ROS Service class generated from running `catkin_make` command -- The tcp server that will connect to Unity +- The ROS node name + +```python + tcp_server = TcpServer(ros_node_name) +``` + +## Source Destination Dictionary + +Create a dictionary keyed by topic or service with the corresponding ROS communication class as the value. The dictionary is used by the TCP server to direct messages to and from the ROS network. + +```python + tcp_server.source_destination_dict = { + 'pos_srv': RosService('position_service', PositionService), + 'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10), + 'color': RosSubscriber('color', Color, tcp_server), + } +``` -`UnityService('obj_pose_srv', ObjectPoseService, tcp_server)` +## Starting the Server +```python + tcp_server.start() + rospy.spin() + +``` -## Parameters The following parameters can be hardcoded, but for the sake of portability, we recommend setting the parameters using the `rosparam set` command, or a `rosparam` YAML file. @@ -139,9 +130,9 @@ An example launch file that will set the appropriate ROSPARAM values required fo ``` - + - + @@ -151,4 +142,4 @@ An example launch file that will set the appropriate ROSPARAM values required fo -``` \ No newline at end of file +``` From 2702a7caf02727a3ccdd59cfb963fb8530ebfbbe Mon Sep 17 00:00:00 2001 From: vidurvij-Unity <60901103+vidurvij-Unity@users.noreply.github.com> Date: Mon, 4 Jan 2021 09:23:40 -0800 Subject: [PATCH 02/35] Updating instructions for URDF repository (#71) * 1. Adding images for URDF tutorial 2. Adding instructions highlighting Disable Collison tag * Adding link to example of disable collision tag in the niryo_one URDF file --- tutorials/urdf_importer/urdf_appendix.md | 44 ++++++++++++------------ 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/tutorials/urdf_importer/urdf_appendix.md b/tutorials/urdf_importer/urdf_appendix.md index 9fd0e8c4..4c6129d7 100644 --- a/tutorials/urdf_importer/urdf_appendix.md +++ b/tutorials/urdf_importer/urdf_appendix.md @@ -3,9 +3,9 @@ ## File Hierarchy URDF files and associated meshes should be placed in a single folder within the Assets directory of Unity. We suggest creating a new folder with the robot's name and place the URDF file in its root with all associated mesh files in sub folders. Be sure to update the file locations as described by the URDF file. -## GameObject Hierarchy +## GameObject Hierarchy -The robot imported in Unity follows a Parent - Child hierarchy. +The robot imported in Unity follows a Parent - Child hierarchy. - Robot GameObject - UrdfRobot Script: UrdfRobot.cs script contains functions to change the behaviors of the imported robot. The script is used to control a robot's behavior, compare against a URDF file, and export the robot as a URDF file. - Controller Script: Enables keyboard control of the robot. It can be modified to create custom controllers as described [here](##Guide-to-write-your-own-controller). @@ -30,12 +30,12 @@ URDF comparator is a testing tool to compare two URDF files. This tool can be us - After the exported file location is selected, its location is automatically selected as location of `Log File Save Location`. You can choose your own save location by pressing the `Select` next to the text field. - To compare the two URDF files press `Compare URDF Files` to generate the [log file](#URDF_Comparator). -## URDF_Comparator +## URDF_Comparator The URDF comparator generates a log file which details the results of the comparison. Log file contains two types of log blocks. One type of block details joint: ``` ********LINK***** - Name: + Name: Equal: True Name: Source: base_link Exported: base_link Inertial Checks @@ -64,10 +64,10 @@ The URDF comparator generates a log file which details the results of the compar Material Checks Material Nullity Check: True Name Equal: True - Name: Source: LightGrey + Name: Source: LightGrey Exported: LightGrey Colors Equal: True - RGB :0.700 0.700 0.700 + RGB :0.700 0.700 0.700 Texture nullity equality: True Collisions Checks Number of Collision Components @@ -77,8 +77,8 @@ Exported Count: 0001 -Collision Component: 1 Collision Name Equal: True - Name: Source: - Exported: + Name: Source: + Exported: Origin Checks Origin Nullity Check: True Geometry: @@ -118,9 +118,9 @@ Parent: Exported: base_link Axis Checks Axis Equal: True - XYZ : (0.000,0.000,1.000) + XYZ : (0.000,0.000,1.000) Dynamics Checks - Dynamics Equal: True + Dynamics Equal: True Lower Limit: Equal: True Lower Limit Value: -3.14159265359 @@ -135,7 +135,7 @@ Child Name: Source: shoulder_link Child Name: Exported: shoulder_link ``` -Log file contains comparison for every attribute contained in the source URDF file to the corresponding attribute in exported URDF file. Each attribute follows the format : +Log file contains comparison for every attribute contained in the source URDF file to the corresponding attribute in exported URDF file. Each attribute follows the format : ``` Equal : @@ -144,7 +144,7 @@ Equal : ``` Some attributes generates a nullity check which checks for presence of attributes in both URDF files. -## Articulation Body axis definition +## Articulation Body axis definition ![](images/Unity_coordinate.png) The most commonly used Coordinate system is the right hand coordinate system which is shown in the figure above. It follows the convention that if your right-hand thumb represents the X direction, then the index finger and the middle finger stretched out uncrossed at right angles would represent the Y and Z direction respectively. Positive direction of rotation is represented by the curl of the fingers with the thumb stretched out. @@ -163,7 +163,7 @@ Note: When the mesh is imported in Unity, the x axis is negated to convert the m Articulation Body allows the joints to be controlled by three methods: 1. Position Control -2. Torque Control +2. Torque Control 3. Velocity Control All types of control are governed by the Spring-Damper equation : @@ -174,7 +174,7 @@ F = stiffness * (currentPosition - target) - damping * (currentVelocity - target #### Example Code -Code for positional control can be divided into two scripts. One script would be attached to the root GameObject which represents the robot. This script would determine the correct position of the joint, directed by a control policy implemented by the user. The other end of the script is attached to the GameObject with ArticulationBody component which receives the target position and assigns that +Code for positional control can be divided into two scripts. One script would be attached to the root GameObject which represents the robot. This script would determine the correct position of the joint, directed by a control policy implemented by the user. The other end of the script is attached to the GameObject with ArticulationBody component which receives the target position and assigns that value to the articulation body. **Note: This architecture can be changed according to the user's design.** @@ -215,7 +215,7 @@ void Update() } Highlight(selectedIndex); } - + UpdateDirection(selectedIndex); } ``` @@ -224,10 +224,10 @@ The up and down arrow keys are used to update the direction in which the joint i private void UpdateDirection(int jointIndex) { float moveDirection = Input.GetAxis("Vertical"); - JointControl current = articulationChain[jointIndex].GetComponent(); + JointControl current = articulationChain[jointIndex].GetComponent(); if (previousIndex != jointIndex) { - JointControl previous = articulationChain[previousIndex].GetComponent(); + JointControl previous = articulationChain[previousIndex].GetComponent(); previous.direction = RotationDirection.None; previousIndex = jointIndex; } @@ -269,16 +269,16 @@ The joint control script determines the direction in which the joint needs to mo float newTargetDelta = (int)direction * Time.fixedDeltaTime * speed; if (newTargetDelta + currentDrive.target <= currentDrive.upperLimit && newTargetDelta + currentDrive.target >= currentDrive.lowerLimit){ currentDrive.target += newTargetDelta; - } + } joint.xDrive = currentDrive; - } + } } } } ``` -### Position Control +### Position Control Position Control can be imagined as adding a spring to a joint with linear spring in case of prismatic joint and a torsional spring in case of revolute joint. The joint follows Hooke's law: @@ -317,10 +317,10 @@ To address this predicament we have integrated another algorithm to create Conve ![](images/ConvexMeshComparison.png) ## Disable Collision Support -URDF defines the robot visually using Visual Meshes, and its collision using Collision Meshes. Collision meshes define the physical volume of the links, and are used to calculate the inertia of the links and also to detect collisions between different physical objects. In Unity, rigidbodies cannot have concave collision meshes, so when importing a concave collision mesh, all concave regions are closed over to produce a convex outline. As a result, the convex shapes might intersect with each other, creating a hindrance in robot movement. To remedy this, we support a ```disable collision``` tag in URDF. To add an exception for collision detection in Unity: +URDF defines the robot visually using Visual Meshes and its inertial volume using collision meshes. Inertial meshes used to define the physical volume of the links and help in calculating the inertia of the links and detecting the collisions between different physical objects. When a collider mesh is imported in Unity, it is decomposed into near convex shapes to form a concave hull. This is necessary in detecting collisions between two mesh colliders. The changed shape might intersect with each other creating a hindrance in robot movement. To remedy this, we support a ```disable collision``` tag in URDF. To add an exception for collision detection in Unity: 1. Identify the links between which you want to ignore the collisions. -2. Add a tag in the URDF file with the format +2. Add a tag in the URDF file with the format ```XML link2=> From 926cdf6fd5e6ebef540866134dc272203ed539e2 Mon Sep 17 00:00:00 2001 From: sarah-gibson <72100462+sarah-gibson@users.noreply.github.com> Date: Wed, 3 Feb 2021 11:18:47 -0800 Subject: [PATCH 03/35] Sarah/readme update (#112) * Adding physics roadmap and minor formatting changes * formatting nits * Calling out feature requests --- README.md | 29 +++++++---------------------- 1 file changed, 7 insertions(+), 22 deletions(-) diff --git a/README.md b/README.md index fe327a8c..177af0b6 100644 --- a/README.md +++ b/README.md @@ -2,8 +2,6 @@ # Unity Robotics Hub -[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) - This is a central repository for tools, tutorials, resources, and documentation for robotic simulation in Unity. > The contents of this repository are in active development. Its features and API are subject to significant change as development progresses. @@ -19,7 +17,7 @@ Simulation plays an important role in robotics development, and we’re here to | [Quick Installation Instructions](tutorials/quick_setup.md) | Brief steps on installing the Unity Robotics packages | | [Pick-and-Place Demo](tutorials/pick_and_place/README.md) | A complete end-to-end demonstration, including how to set up the Unity environment, how to import a robot from URDF, and how to set up two-way communication with ROS for control | | [ROS–Unity Integration](tutorials/ros_unity_integration/README.md) | A set of component-level tutorials showing how to set up communication between ROS and Unity | -| [URDF Importer](tutorials/urdf_importer/urdf_tutorial.md) | Steps on using the Unity package for loading [URDF](http://wiki.ros.org/urdf) files | +| [URDF Importer](tutorials/urdf_importer/urdf_tutorial.md) | Steps on using the Unity package for loading [URDF](http://wiki.ros.org/urdf) files | | [Articulations Robot Demo](https://github.com/Unity-Technologies/articulations-robot-demo) | A robot simulation demonstrating Unity's new physics solver (no ROS dependency) @@ -43,42 +41,29 @@ Unite Now 2020: Simulating Robots with ROS and Unity [video](https://resources.u - (August 26, 2020) Announcing Unity Robotic Simulation [blog post](https://unity.com/solutions/automotive-transportation-manufacturing/robotics) - (May 20, 2020) -Use articulation bodies to easily prototype industrial designs with realistic motion and behavior [blog post](https://blogs.unity3d.com/2020/05/20/use-articulation-bodies-to-easily-prototype-industrial-designs-with-realistic-motion-and-behavior/) +Use articulation bodies to easily prototype industrial designs with realistic motion and behavior [blog post](https://blogs.unity3d.com/2020/05/20/use-articulation-bodies-to-easily-prototype-industrial-designs-with-realistic-motion-and-behavior/) ### More from Unity - [Unity Industrial Simulation](https://unity.com/products/unity-simulation) - [Unity Computer Vision](https://unity.com/computer-vision) -## New Physics Features in Unity -### New Features -- **Contact Modification API** This API will allow users to define custom contact reactions, such as ignoring subsets of contact points, in order to help simulate holes, slippery surfaces, soft contacts, and more. It is available in Unity versions **2021.2a12+**. [Read more about the new Contact Modification API](https://forum.unity.com/threads/experimental-contacts-modification-api.924809/). -- **Collision detection modes exposed for ArticulationBody: discrete, sweep-based CCD, and speculative CCD**. New continuous collision detection (CCD) modes will ensure that fast-moving objects collide with objects, instead of tunneling or passing through those objects, which can happen in the default “discrete” mode. This API is available in Unity versions **2020.3.5f1+**. [Read more about continuous collision detection](https://docs.unity3d.com/2020.3/Documentation/ScriptReference/ArticulationBody-collisionDetectionMode.html). - -### Coming Soon +## Coming Soon - New Physics Features in Unity! Here’s a peek into what our Physics Team is hard at work on… +- **Contact Modification API**. This API will allow users to define custom contact reactions, such as ignoring subsets of contact points, in order to help simulate holes, slippery surfaces, soft contacts, and more. [Read more about the new Contact Modification API](https://forum.unity.com/threads/experimental-contacts-modification-api.924809/). - **Wheel Collider shapes**. This feature will allow the user to specify the shape of the collider to be used for collision detection. Currently the collider shape is fixed to a cylinder, and collision detection is performed by casting a ray from the center of the cylinder. Custom shapes will improve the accuracy of simulating wheels over rough terrains, holes, etc. [Read more about Wheel Collider](https://docs.unity3d.com/Manual/class-WheelCollider.html). +- **Collision detection modes exposed for ArticulationBody: discrete, sweep-based ccd, and speculative ccd**. New continuous collision detection (ccd) modes will ensure that fast-moving objects collide with objects, instead of tunneling or passing through those objects, which can happen in the default “discrete” mode. [Read more about continuous collision detection](https://docs.unity3d.com/Manual/ContinuousCollisionDetection.html). - **Force/Torque Sensor API**. This API will allow users to get the force and torque acting on an articulation body (useful for simulating a force/torque sensor!), as well as to get the motor torque applied by an articulation drive. - **Query primitives**. These simple, GameObject-less shapes allow for collision detection without requiring simulation (i.e., without calling Physics.Simulate). This feature will allow users to initialize objects in feasible locations, and can also be used for motion planning. ## FAQs [FAQs](faq.md) -## Community and Feedback - -The Unity Robotics projects are open-source and we encourage and welcome contributions. -If you wish to contribute, be sure to review our [contribution guidelines](CONTRIBUTING.md) -and [code of conduct](CODE_OF_CONDUCT.md). - ## Support +For general questions, feedback, or feature requests, connect directly with the Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). -For questions or discussions about Unity Robotics package installations or how to best set up and integrate your robotics projects, please create a new thread on the [Unity Robotics forum](https://forum.unity.com/forums/robotics.623/) and make sure to include as much detail as possible. - -For feature requests, bugs, or other issues, please file a [GitHub issue](https://github.com/Unity-Technologies/Unity-Robotics-Hub/issues) using the provided templates and the Robotics team will investigate as soon as possible. - -For any other questions or feedback, connect directly with the -Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). +For bugs or other issues, please file a GitHub issue and the Robotics team will investigate the issue as soon as possible. ## License [Apache License 2.0](LICENSE) From 9e45928f6f413f19c5bde460af9092c996cf165c Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Thu, 4 Feb 2021 13:39:28 -0800 Subject: [PATCH 04/35] Update for ROS-TCP-Connector namespace changes --- .../Assets/DemoScripts/Demo.cs | 79 +++++++++---------- .../Packages/manifest.json | 4 +- .../Scripts/SourceDestinationPublisher.cs | 23 +++--- .../Scripts/TrajectoryPlanner.cs | 48 +++++------ .../unity_scripts/RosSubscriberExample.cs | 36 ++++----- 5 files changed, 93 insertions(+), 97 deletions(-) diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs index 0b4a591b..99e9911c 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs +++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs @@ -1,14 +1,14 @@ #if UNITY_EDITOR using Microsoft.CSharp; +using RosSharp; +using RosSharp.Control; +using RosSharp.Urdf.Editor; using System; using System.CodeDom.Compiler; using System.Collections.Generic; using System.IO; using System.Reflection; using System.Text; -using RosSharp; -using RosSharp.Control; -using RosSharp.Urdf.Editor; using UnityEditor; using UnityEngine; using Unity.Robotics.ROSTCPConnector; @@ -75,10 +75,8 @@ void Awake() { EditorApplication.LockReloadAssemblies(); SetupScene(); - AddRosMessages(); - ImportRobot(); - CreateRosConnection(); - CreateTrajectoryPlannerPublisher(); + SetupRos(); + CreateTrajectoryPlannerPubliser(); } void Update() @@ -106,18 +104,31 @@ void OnApplicationQuit() private void SetupScene() { // Instantiate the table, target, and target placement - InstantiatePrefab(tableName); - InstantiatePrefab(targetName); - InstantiatePrefab(targetPlacementName); + GameObject table = InstantiatePrefab(tableName); + GameObject target = InstantiatePrefab(targetName); + GameObject targetPlacement = InstantiatePrefab(targetPlacementName); // Adjust main camera GameObject camera = GameObject.Find(cameraName); camera.transform.position = cameraPosition; camera.transform.rotation = cameraRotation; + + // Import Niryo One with URDF importer + string urdfFilepath = Path.Combine(Application.dataPath, urdfRelativeFilepath); + ImportRobot(urdfFilepath, ImportSettings.convexDecomposer.unity); + + // Adjust robot parameters + Controller controller = GameObject.Find(niryoOneName).GetComponent(); + controller.stiffness = controllerStiffness; + controller.damping = controllerDamping; + controller.forceLimit = controllerForceLimit; + controller.speed = controllerSpeed; + controller.acceleration = controllerAcceleration; + GameObject.Find(baseLinkName).GetComponent().immovable = true; } // Tutorial Part 2 without the Publisher object - private void AddRosMessages() + private void SetupRos() { if (generateRosMessages) { @@ -141,9 +152,19 @@ private void AddRosMessages() scripts.AddRange(Directory.GetFiles(rosMessagesDirectory, scriptPattern, SearchOption.AllDirectories)); scripts.AddRange(Directory.GetFiles(externalScriptsDirectory)); RecompileScripts(scripts.ToArray()); + + // Create RosConnect + GameObject rosConnect = new GameObject(rosConnectName); + rosConnection = rosConnect.AddComponent(); + rosConnection.rosIPAddress = hostIP; + rosConnection.rosPort = hostPort; + rosConnection.overrideUnityIP = overrideUnityIP; + rosConnection.unityPort = unityPort; + rosConnection.awaitDataMaxRetries = awaitDataMaxRetries; + rosConnection.awaitDataSleepSeconds = awaitDataSleepSeconds; } - private void CreateTrajectoryPlannerPublisher() + private void CreateTrajectoryPlannerPubliser() { GameObject publisher = new GameObject(publisherName); dynamic planner = publisher.AddComponent(assembly.GetType(trajectoryPlannerType)); @@ -161,39 +182,15 @@ private GameObject InstantiatePrefab(string name) return gameObject; } - private void CreateRosConnection() - { - // Create RosConnect - GameObject rosConnect = new GameObject(rosConnectName); - rosConnection = rosConnect.AddComponent(); - rosConnection.rosIPAddress = hostIP; - rosConnection.rosPort = hostPort; - rosConnection.overrideUnityIP = overrideUnityIP; - rosConnection.unityPort = unityPort; - rosConnection.awaitDataMaxRetries = awaitDataMaxRetries; - rosConnection.awaitDataSleepSeconds = awaitDataSleepSeconds; - } - - private void ImportRobot() + private void ImportRobot(string urdfFilepath, ImportSettings.convexDecomposer convexDecomposer) { - ImportSettings urdfImportSettings = new ImportSettings + // Import Niryo One by URDF Importer + ImportSettings settings = new ImportSettings { choosenAxis = ImportSettings.axisType.yAxis, - convexMethod = ImportSettings.convexDecomposer.unity + convexMethod = convexDecomposer, }; - // Import Niryo One with URDF importer - string urdfFilepath = Path.Combine(Application.dataPath, urdfRelativeFilepath); - // Create is a coroutine that would usually run only in EditMode, so we need to force its execution here - var robotImporter = UrdfRobotExtensions.Create(urdfFilepath, urdfImportSettings, false); - while (robotImporter.MoveNext()) {} - // Adjust robot parameters - Controller controller = GameObject.Find(niryoOneName).GetComponent(); - controller.stiffness = controllerStiffness; - controller.damping = controllerDamping; - controller.forceLimit = controllerForceLimit; - controller.speed = controllerSpeed; - controller.acceleration = controllerAcceleration; - GameObject.Find(baseLinkName).GetComponent().immovable = true; + UrdfRobotExtensions.Create(urdfFilepath, settings); } // Credit to https://www.codeproject.com/Tips/715891/Compiling-Csharp-Code-at-Runtime diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json index 20a48000..ffb4a201 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json +++ b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json @@ -4,8 +4,8 @@ "com.unity.ide.rider": "2.0.7", "com.unity.ide.visualstudio": "2.0.3", "com.unity.ide.vscode": "1.2.2", - "com.unity.robotics.urdf-importer": "https://github.com/Unity-Technologies/URDF-Importer.git#v0.0.4", - "com.unity.robotics.ros-tcp-connector": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git#v0.1.1", + "com.unity.robotics.ros-tcp-connector": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#0b7ddbf36a13fbd47a0aeead062f0f57271b7e96", + "com.unity.robotics.urdf-importer": "https://github.com/Unity-Technologies/URDF-Importer.git#v0.1.2", "com.unity.test-framework": "1.1.18", "com.unity.textmeshpro": "3.0.1", "com.unity.timeline": "1.4.3", diff --git a/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs b/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs index 7fc2fab5..f0aa675c 100644 --- a/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs +++ b/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs @@ -2,26 +2,25 @@ using UnityEngine; using Unity.Robotics.ROSTCPConnector; using Unity.Robotics.ROSTCPConnector.ROSGeometry; -using RosMessageTypes.Geometry; public class SourceDestinationPublisher : MonoBehaviour { // ROS Connector private ROSConnection ros; - + // Variables required for ROS communication public string topicName = "SourceDestination_input"; public GameObject niryoOne; public GameObject target; public GameObject targetPlacement; - + private int numRobotJoints = 6; private readonly Quaternion pickOrientation = Quaternion.Euler(90, 90, 0); - + // Articulation Bodies private ArticulationBody[] jointArticulationBodies; - + /// /// /// @@ -36,23 +35,23 @@ void Start() string arm_link = shoulder_link + "/arm_link"; jointArticulationBodies[1] = niryoOne.transform.Find(arm_link).GetComponent(); - + string elbow_link = arm_link + "/elbow_link"; jointArticulationBodies[2] = niryoOne.transform.Find(elbow_link).GetComponent(); - + string forearm_link = elbow_link + "/forearm_link"; jointArticulationBodies[3] = niryoOne.transform.Find(forearm_link).GetComponent(); - + string wrist_link = forearm_link + "/wrist_link"; jointArticulationBodies[4] = niryoOne.transform.Find(wrist_link).GetComponent(); - + string hand_link = wrist_link + "/hand_link"; jointArticulationBodies[5] = niryoOne.transform.Find(hand_link).GetComponent(); } public void Publish() { - MNiryoMoveitJoints sourceDestinationMessage = new MNiryoMoveitJoints(); + NiryoMoveitJoints sourceDestinationMessage = new NiryoMoveitJoints(); sourceDestinationMessage.joint_00 = jointArticulationBodies[0].xDrive.target; sourceDestinationMessage.joint_01 = jointArticulationBodies[1].xDrive.target; @@ -62,14 +61,14 @@ public void Publish() sourceDestinationMessage.joint_05 = jointArticulationBodies[5].xDrive.target; // Pick Pose - sourceDestinationMessage.pick_pose = new MPose + sourceDestinationMessage.pick_pose = new RosMessageTypes.Geometry.Pose { position = target.transform.position.To(), orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To() }; // Place Pose - sourceDestinationMessage.place_pose = new MPose + sourceDestinationMessage.place_pose = new RosMessageTypes.Geometry.Pose { position = targetPlacement.transform.position.To(), orientation = pickOrientation.To() diff --git a/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs b/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs index a9b57d2e..f14015a1 100644 --- a/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs +++ b/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs @@ -4,7 +4,7 @@ using UnityEngine; using Unity.Robotics.ROSTCPConnector; using Unity.Robotics.ROSTCPConnector.ROSGeometry; -using RosMessageTypes.Geometry; + public class TrajectoryPlanner : MonoBehaviour { @@ -16,7 +16,7 @@ public class TrajectoryPlanner : MonoBehaviour private readonly float jointAssignmentWait = 0.1f; private readonly float poseAssignmentWait = 0.5f; private readonly Vector3 pickPoseOffset = Vector3.up * 0.1f; - + // Assures that the gripper is always positioned above the target cube before grasping. private readonly Quaternion pickOrientation = Quaternion.Euler(90, 90, 0); @@ -43,7 +43,7 @@ private enum Poses PickUp, Place }; - + /// /// Close the gripper /// @@ -78,10 +78,10 @@ private void OpenGripper() /// Get the current values of the robot's joint angles. /// /// NiryoMoveitJoints - MNiryoMoveitJoints CurrentJointConfig() + NiryoMoveitJoints CurrentJointConfig() { - MNiryoMoveitJoints joints = new MNiryoMoveitJoints(); - + NiryoMoveitJoints joints = new NiryoMoveitJoints(); + joints.joint_00 = jointArticulationBodies[0].xDrive.target; joints.joint_01 = jointArticulationBodies[1].xDrive.target; joints.joint_02 = jointArticulationBodies[2].xDrive.target; @@ -101,11 +101,11 @@ MNiryoMoveitJoints CurrentJointConfig() /// public void PublishJoints() { - MMoverServiceRequest request = new MMoverServiceRequest(); + MoverServiceRequest request = new MoverServiceRequest(); request.joints_input = CurrentJointConfig(); - + // Pick Pose - request.pick_pose = new MPose + request.pick_pose = new RosMessageTypes.Geometry.Pose { position = (target.transform.position + pickPoseOffset).To(), // The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping. @@ -113,16 +113,16 @@ public void PublishJoints() }; // Place Pose - request.place_pose = new MPose + request.place_pose = new RosMessageTypes.Geometry.Pose { position = (targetPlacement.transform.position + pickPoseOffset).To(), orientation = pickOrientation.To() }; - ros.SendServiceMessage(rosServiceName, request, TrajectoryResponse); + ros.SendServiceMessage(rosServiceName, request, TrajectoryResponse); } - void TrajectoryResponse(MMoverServiceResponse response) + void TrajectoryResponse(MoverServiceResponse response) { if (response.trajectories.Length > 0) { @@ -149,23 +149,23 @@ void TrajectoryResponse(MMoverServiceResponse response) /// /// MoverServiceResponse received from niryo_moveit mover service running in ROS /// - private IEnumerator ExecuteTrajectories(MMoverServiceResponse response) + private IEnumerator ExecuteTrajectories(MoverServiceResponse response) { if (response.trajectories != null) { // For every trajectory plan returned - for (int poseIndex = 0; poseIndex < response.trajectories.Length; poseIndex++) + for (int poseIndex = 0 ; poseIndex < response.trajectories.Length; poseIndex++) { // For every robot pose in trajectory plan - for (int jointConfigIndex = 0; jointConfigIndex < response.trajectories[poseIndex].joint_trajectory.points.Length; jointConfigIndex++) + for (int jointConfigIndex = 0 ; jointConfigIndex < response.trajectories[poseIndex].joint_trajectory.points.Length; jointConfigIndex++) { var jointPositions = response.trajectories[poseIndex].joint_trajectory.points[jointConfigIndex].positions; - float[] result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray(); - + float[] result = jointPositions.Select(r=> (float)r * Mathf.Rad2Deg).ToArray(); + // Set the joint values for every joint for (int joint = 0; joint < jointArticulationBodies.Length; joint++) { - var joint1XDrive = jointArticulationBodies[joint].xDrive; + var joint1XDrive = jointArticulationBodies[joint].xDrive; joint1XDrive.target = result[joint]; jointArticulationBodies[joint].xDrive = joint1XDrive; } @@ -176,7 +176,7 @@ private IEnumerator ExecuteTrajectories(MMoverServiceResponse response) // Close the gripper if completed executing the trajectory for the Grasp pose if (poseIndex == (int)Poses.Grasp) CloseGripper(); - + // Wait for the robot to achieve the final pose from joint assignment yield return new WaitForSeconds(poseAssignmentWait); } @@ -200,16 +200,16 @@ void Start() string arm_link = shoulder_link + "/arm_link"; jointArticulationBodies[1] = niryoOne.transform.Find(arm_link).GetComponent(); - + string elbow_link = arm_link + "/elbow_link"; jointArticulationBodies[2] = niryoOne.transform.Find(elbow_link).GetComponent(); - + string forearm_link = elbow_link + "/forearm_link"; jointArticulationBodies[3] = niryoOne.transform.Find(forearm_link).GetComponent(); - + string wrist_link = forearm_link + "/wrist_link"; jointArticulationBodies[4] = niryoOne.transform.Find(wrist_link).GetComponent(); - + string hand_link = wrist_link + "/hand_link"; jointArticulationBodies[5] = niryoOne.transform.Find(hand_link).GetComponent(); @@ -225,4 +225,4 @@ void Start() rightGripper = rightGripperGameObject.GetComponent(); leftGripper = leftGripperGameObject.GetComponent(); } -} +} \ No newline at end of file diff --git a/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs b/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs index c36a3da3..7040b625 100644 --- a/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs +++ b/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs @@ -1,18 +1,18 @@ -using UnityEngine; -using Unity.Robotics.ROSTCPConnector; -using RosColor = RosMessageTypes.RoboticsDemo.MUnityColor; - -public class RosSubscriberExample : MonoBehaviour -{ - public GameObject cube; - - void Start() - { - ROSConnection.instance.Subscribe("color", ColorChange); - } - - void ColorChange(RosColor colorMessage) - { - cube.GetComponent().material.color = new Color32((byte)colorMessage.r, (byte)colorMessage.g, (byte)colorMessage.b, (byte)colorMessage.a); - } -} +using UnityEngine; +using Unity.Robotics.ROSTCPConnector; +using RosColor = RosMessageTypes.RoboticsDemo.UnityColor; + +public class RosSubscriberExample : MonoBehaviour +{ + public GameObject cube; + + void Start() + { + ROSConnection.instance.Subscribe("color", ColorChange); + } + + void ColorChange(RosColor colorMessage) + { + cube.GetComponent().material.color = new Color32((byte)colorMessage.r, (byte)colorMessage.g, (byte)colorMessage.b, (byte)colorMessage.a); + } +} From 2b17c1ea31b43fc1955348a9f925eb76e93ec549 Mon Sep 17 00:00:00 2001 From: Shuo Diao Date: Mon, 8 Feb 2021 08:54:48 -0800 Subject: [PATCH 05/35] Mpinol/simrealrebased (#107) * Adding sim and real pnp files. --- tutorials/pick_and_place/4_pick_and_place.md | 40 ++++++++----------- tutorials/pick_and_place/README.md | 8 ++-- .../niryo_moveit/scripts/server_endpoint.py | 19 +++++---- .../Scripts_Part4/RealSimPickAndPlace.cs | 32 +++++++-------- 4 files changed, 46 insertions(+), 53 deletions(-) diff --git a/tutorials/pick_and_place/4_pick_and_place.md b/tutorials/pick_and_place/4_pick_and_place.md index dad4e25f..047bb14a 100644 --- a/tutorials/pick_and_place/4_pick_and_place.md +++ b/tutorials/pick_and_place/4_pick_and_place.md @@ -16,7 +16,7 @@ This part is going to be a little different than the previous tutorials in that - [Add niryo_moveit Package](#add-niryo_moveit-package) - [Execution](#execution) - + # Niryo One Information The best source of information for the Niryo One is the [User Manual](https://niryo.com/docs/niryo-one/user-manual/complete-user-manual/). It contains a lot of general information about the Niryo One, such as how to connect it to a network, how to log in to it, and how to use the Niryo One Studio desktop application. @@ -115,7 +115,7 @@ command_list: ``` -From here we see that the `RobotMoveGoal.RobotMoveCommand.ToolCommand.cmd_type` variable will need to be `2` to close the gripper and `1` to open it. +From here we see that the `RobotMoveGoal.RobotMoveCommand.ToolCommand.cmd_type` variable will need to be `2` to close the gripper and `1` to open it. # Differences From Part 3 @@ -173,20 +173,20 @@ We will also make use of the `sim_and_real_pnp.py` script. It is very similar to ```python def send_trajectory_goal(client, trajectory): - + # Build the goal goal = RobotMoveGoal() goal.cmd.Trajectory = trajectory goal.cmd.cmd_type = TRAJECTORY_COMMAND_ID - + client.send_goal(goal) client.wait_for_result() - + return ``` - To send a gripper command: - + ```python def send_tool_goal(client, gripper_command): tool_command = ToolCommand() @@ -194,35 +194,35 @@ We will also make use of the `sim_and_real_pnp.py` script. It is very similar to tool_command.cmd_type = gripper_command tool_command.gripper_open_speed = GRIPPER_SPEED tool_command.gripper_close_speed = GRIPPER_SPEED - + goal = RobotMoveGoal() goal.cmd.tool_cmd = tool_command goal.cmd.cmd_type = TOOL_COMMAND_ID - + client.send_goal(goal) client.wait_for_result() - + return ``` - The `pick_and_place` function has been updated to call the two previous functions instead of appending the trajectory or gripper command to a list. - Example from `mover.py` - + ```python previous_ending_joint_angles = grasp_pose.joint_trajectory.points[-1].positions response.trajectories.append(grasp_pose) ``` - + - Updated code in `sim_real_pnp.py` - + ```python previous_ending_joint_angles = grasp_pose.trajectory.joint_trajectory.points[-1].positions send_trajectory_goal(client, grasp_pose) ``` # The Unity Side -Using the same scene from [Part 3](3_pick_and_place.md), we are going to use a new script, `RealSimPickAndPlace.cs`, that mirrors a lot of the functionality of the `TrajectoryPlanner.cs` script. +Using the same scene from [Part 3](3_pick_and_place.md), we are going to use a new script, `RealSimPickAndPlace.cs`, that mirrors a lot of the functionality of the `TrajectoryPlanner.cs` script. ## Key Differences @@ -272,7 +272,7 @@ void Start() { var jointPositions = point.positions; float[] result = jointPositions.Select(r=> (float)r * Mathf.Rad2Deg).ToArray(); - + // Set the joint values for every joint for (int joint = 0; joint < jointArticulationBodies.Length; joint++) { @@ -295,17 +295,11 @@ void Start() ## Setting Up The Unity Scene 1. In Unity, Select Robotics -> ROS Settings from the top menu bar and update the ROS IP Address to that of the Niryo One. -1. Select "Robotics/Generate ROS Messages..." from the menu and build the messages you will need - Build the contents of the moveit_msgs/msg folder, the niryo_one_ros/niryo_one_msgs/action folder and the niryo_one_ros/niryo_one_msgs/msg folder. - - ![](img/4_build_niryo_msgs.png) - -1. Find PATH/TO/Unity-Robotics-Hub/tutorials/pick_and_place/Scripts_Part4 and copy into your project's Assets folder. - 1. Select the Publisher GameObject and add the `RealSimPickAndPlace` script as a component. -1. Note that the RealSimPickAndPlace component shows its member variables in the Inspector window, which need to be assigned. +1. Note that the RealSimPickAndPlace component shows its member variables in the Inspector window, which need to be assigned. - Once again, drag and drop the `Target` and `TargetPlacement` objects onto the Target and Target Placement Inspector fields, respectively. Assign the `niryo_one` robot to the Niryo One field. + Once again, drag and drop the `Target` and `TargetPlacement` objects onto the Target and Target Placement Inspector fields, respectively. Assign the `niryo_one` robot to the Niryo One field. ![](img/4_script.png) @@ -325,7 +319,7 @@ void Start() - The two files that will need to be updated are `niryo_one.urdf.xacro` and `without_mesh_niryo_one.urdf.xacro` located in the `/home/niryo/catkin_ws/src/niryo_one_description/urdf/v2` directory. - Look for the joint named `joint_world` and update the `origin`'s `xyz` to `0 0 0.63` to reflect that the simulated Niryo is placed at `0.63` on the Z axis. - + ```xml diff --git a/tutorials/pick_and_place/README.md b/tutorials/pick_and_place/README.md index 3e5edd77..61f60818 100644 --- a/tutorials/pick_and_place/README.md +++ b/tutorials/pick_and_place/README.md @@ -2,7 +2,7 @@ # Pick-and-Place Tutorial -Unity's tools for robotic simulation enable users to integrate Unity with ROS-based workflows. [ROS](http://wiki.ros.org/ROS/Introduction) (Robot Operating System) provides services such as message-passing, package management, low-level device control, and hardware abstraction. Unity's robotics tools are able to support **importing URDF files** and **sending and receiving messages between ROS and Unity**. This tutorial will go through the steps necessary to integrate ROS with Unity, from installing the Unity Editor to creating a scene with an imported URDF to completing a pick-and-place task with known poses using [MoveIt](https://moveit.ros.org/) trajectory planning. +Unity's tools for robotic simulation enable users to integrate Unity with ROS-based workflows. [ROS](http://wiki.ros.org/ROS/Introduction) (Robot Operating System) provides services such as message-passing, package management, low-level device control, and hardware abstraction. Unity's robotics tools are able to support **importing URDF files** and **sending and receiving messages between ROS and Unity**. This tutorial will go through the steps necessary to integrate ROS with Unity, from installing the Unity Editor to creating a scene with an imported URDF to completing a pick-and-place task with known poses using [MoveIt](https://moveit.ros.org/) trajectory planning. This tutorial is designed such that you do not need prior experience with Unity or C# in order to follow the scene setup steps, and you do not need prior robotics experience to get started with ROS integration. The tutorial is divided into high-level phases, from basic Unity and ROS initial setup through executing a pick-and-place task. @@ -31,11 +31,11 @@ This repository provides project files for the pick-and-place tutorial, includin This part provides two options for setting up your ROS workspace: using Docker, or manually setting up a catkin workspace. -## [Part 1: Create Unity scene with imported URDF](1_urdf.md) +## [Part 1: Create Unity scene with imported URDF](1_urdf.md) -This part includes downloading and installing the Unity Editor, setting up a basic Unity scene, and importing a robot--the [Niryo One](https://niryo.com/niryo-one/)--using the URDF Importer. +This part includes downloading and installing the Unity Editor, setting up a basic Unity scene, and importing a robot--the [Niryo One](https://niryo.com/niryo-one/)--using the URDF Importer. ## [Part 2: ROS–Unity Integration](2_ros_tcp.md) @@ -44,7 +44,7 @@ This part includes downloading and installing the Unity Editor, setting up a bas This part covers creating a TCP connection between Unity and ROS, generating C# scripts from a ROS msg and srv files, and publishing to a ROS topic. ## [Part 3: Pick-and-Place In Unity](3_pick_and_place.md) - + This part includes the preparation and setup necessary to run a pick-and-place task with known poses using MoveIt. Steps covered include creating and invoking a motion planning service in ROS, moving a Unity Articulation Body based on a calculated trajectory, and controlling a gripping tool to successfully grasp and drop an object. diff --git a/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py b/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py index 53674b09..3dc74ebe 100755 --- a/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py +++ b/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py @@ -3,25 +3,24 @@ import rospy from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService - from niryo_moveit.msg import NiryoMoveitJoints, NiryoTrajectory -from niryo_moveit.srv import MoverService +from niryo_moveit.srv import MoverService, MoverServiceRequest +from niryo_one_msgs.msg import RobotMoveActionGoal def main(): ros_node_name = rospy.get_param("/TCP_NODE_NAME", 'TCPServer') tcp_server = TcpServer(ros_node_name) + rospy.init_node(ros_node_name, anonymous=True) - # Create ROS communication objects dictionary for routing messages - tcp_server.source_destination_dict = { + # Start the Server Endpoint with a ROS communication objects dictionary for routing messages + tcp_server.start({ 'SourceDestination_input': RosPublisher('SourceDestination', NiryoMoveitJoints, queue_size=10), 'NiryoTrajectory': RosSubscriber('NiryoTrajectory', NiryoTrajectory, tcp_server), - 'niryo_moveit': RosService('niryo_moveit', MoverService) - } - - # Start the Server Endpoint - rospy.init_node(ros_node_name, anonymous=True) - tcp_server.start() + 'niryo_moveit': RosService('niryo_moveit', MoverService), + 'niryo_one/commander/robot_action/goal': RosSubscriber('niryo_one/commander/robot_action/goal', RobotMoveActionGoal, tcp_server), + 'sim_real_pnp': RosPublisher('sim_real_pnp', MoverServiceRequest) + }) rospy.spin() diff --git a/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs b/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs index a02eef5a..3e9a32f8 100644 --- a/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs +++ b/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs @@ -1,12 +1,12 @@ using System.Collections; using System.Linq; -using RosMessageTypes.Geometry; +using ROSGeometry; using RosMessageTypes.Moveit; using RosMessageTypes.NiryoMoveit; using RosMessageTypes.NiryoOne; -using Unity.Robotics.ROSTCPConnector; -using Unity.Robotics.ROSTCPConnector.ROSGeometry; using UnityEngine; +using Pose = RosMessageTypes.Geometry.Pose; +using RosQuaternion = RosMessageTypes.Geometry.Quaternion; public class RealSimPickAndPlace : MonoBehaviour { @@ -18,7 +18,7 @@ public class RealSimPickAndPlace : MonoBehaviour private ROSConnection ros; private const int NUM_ROBOT_JOINTS = 6; - // Hardcoded variables + // Hardcoded variables private const float JOINT_ASSIGNMENT_WAIT = 0.038f; private readonly Vector3 PICK_POSE_OFFSET = Vector3.up * 0.15f; @@ -81,7 +81,7 @@ private void OpenGripper() /// public void PublishJoints() { - MMoverServiceRequest request = new MMoverServiceRequest + MoverServiceRequest request = new MoverServiceRequest { joints_input = { @@ -92,13 +92,13 @@ public void PublishJoints() joint_04 = jointArticulationBodies[4].xDrive.target, joint_05 = jointArticulationBodies[5].xDrive.target }, - pick_pose = new MPose + pick_pose = new Pose { position = (target.transform.position + PICK_POSE_OFFSET).To(), // The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping. orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To() }, - place_pose = new MPose + place_pose = new Pose { position = (targetPlacement.transform.position + PICK_POSE_OFFSET).To(), orientation = pickOrientation.To() @@ -146,16 +146,16 @@ void Awake() void Start() { - ros.Subscribe(rosRobotCommandsTopicName, ExecuteRobotCommands); + ros.Subscribe(rosRobotCommandsTopicName, ExecuteRobotCommands); } /// /// Execute robot commands receved from ROS Subscriber. - /// Gripper commands will be executed immeditately wihle trajectories will be + /// Gripper commands will be executed immeditately wihle trajectories will be /// executed in a coroutine. /// /// RobotMoveActionGoal of trajectory or gripper commands - void ExecuteRobotCommands(MRobotMoveActionGoal robotAction) + void ExecuteRobotCommands(RobotMoveActionGoal robotAction) { if (robotAction.goal.cmd.cmd_type == TRAJECTORY_COMMAND_EXECUTION) { @@ -178,24 +178,24 @@ void ExecuteRobotCommands(MRobotMoveActionGoal robotAction) /// /// Execute trajectories from RobotMoveActionGoal topic. - /// - /// Execution will iterate through every robot pose in the trajectory pose + /// + /// Execution will iterate through every robot pose in the trajectory pose /// array while updating the joint values on the simulated robot. - /// + /// /// /// The array of poses for the robot to execute - private IEnumerator ExecuteTrajectories(MRobotTrajectory trajectories) + private IEnumerator ExecuteTrajectories(RobotTrajectory trajectories) { // For every robot pose in trajectory plan foreach (var point in trajectories.joint_trajectory.points) { var jointPositions = point.positions; - float[] result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray(); + float[] result = jointPositions.Select(r=> (float)r * Mathf.Rad2Deg).ToArray(); // Set the joint values for every joint for (int joint = 0; joint < jointArticulationBodies.Length; joint++) { - var joint1XDrive = jointArticulationBodies[joint].xDrive; + var joint1XDrive = jointArticulationBodies[joint].xDrive; joint1XDrive.target = result[joint]; jointArticulationBodies[joint].xDrive = joint1XDrive; } From 4d3e0054f89ce7ac57040bca8c813f512603aad6 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Fri, 12 Feb 2021 14:59:52 -0800 Subject: [PATCH 06/35] Update with new instructions (#147) --- tutorials/pick_and_place/4_pick_and_place.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/tutorials/pick_and_place/4_pick_and_place.md b/tutorials/pick_and_place/4_pick_and_place.md index 047bb14a..3eaa0bea 100644 --- a/tutorials/pick_and_place/4_pick_and_place.md +++ b/tutorials/pick_and_place/4_pick_and_place.md @@ -295,6 +295,12 @@ void Start() ## Setting Up The Unity Scene 1. In Unity, Select Robotics -> ROS Settings from the top menu bar and update the ROS IP Address to that of the Niryo One. +1. Select "Robotics/Generate ROS Messages..." from the menu and build the messages you will need - Build the contents of the moveit_msgs/msg folder, the niryo_one_ros/niryo_one_msgs/action folder and the niryo_one_ros/niryo_one_msgs/msg folder. + + ![](img/4_build_niryo_msgs.png) + +1. Find PATH/TO/Unity-Robotics-Hub/tutorials/pick_and_place/Scripts_Part4 and copy into your project's Assets folder. + 1. Select the Publisher GameObject and add the `RealSimPickAndPlace` script as a component. 1. Note that the RealSimPickAndPlace component shows its member variables in the Inspector window, which need to be assigned. From 2e1c89b999d2183019ed553f5881cc656a5284c6 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Fri, 12 Feb 2021 15:46:11 -0800 Subject: [PATCH 07/35] Update tutorial text (#148) --- tutorials/ros_unity_integration/publisher.md | 37 ++++++++++++------- tutorials/ros_unity_integration/subscriber.md | 13 ++++--- 2 files changed, 31 insertions(+), 19 deletions(-) diff --git a/tutorials/ros_unity_integration/publisher.md b/tutorials/ros_unity_integration/publisher.md index 8ebbf0a6..b50b3e47 100644 --- a/tutorials/ros_unity_integration/publisher.md +++ b/tutorials/ros_unity_integration/publisher.md @@ -12,7 +12,7 @@ Create a simple Unity scene which publishes a GameObject's position and rotation ```bash source devel/setup.bash - rosrun robotics_demo server_endpoint.py + rosrun robotics_demo server_endpoint.py ``` Once the server_endpoint has started, it will print something similar to `[INFO] [1603488341.950794]: Starting server on 192.168.50.149:10000`. @@ -20,38 +20,48 @@ Once the server_endpoint has started, it will print something similar to `[INFO] - Open another new terminal window, navigate to your ROS workspace, and run the following commands: ```bash source devel/setup.bash - rostopic echo pos_rot + rostopic echo pos_rot ``` ## Setting Up Unity Scene -- In the menu bar, find and select `RosMessageGeneration` -> `Auto Generate Messages` -> `Single Message ...` -- Set the input file path to `PATH/TO/Unity-Robotics-Hub/tutorials/ros_packages/robotics_demo/msg/PosRot.msg ` and click `GENERATE!` - - The generated file will be saved in the default directory `Assets/RosMessages/msg` +- In the menu bar, find and select `Robotics` -> `Generate ROS Messages...` +- Set the ROS message path to `PATH/TO/Unity-Robotics-Hub/tutorials/ros_packages/robotics_demo`. + - Expand the robotics_demo subfolder and click "Build 2 msgs" to generate new C# scripts from the ROS .msg files. + +![](images/generate_messages_1.png) + + - The generated files will be saved in the default directory `Assets/RosMessages/RoboticsDemo/msg`. - Create a new directory in `Assets` and name it `Scripts` - Create a new script in the `Scripts` directory and name it `RosPublisherExample.cs` - Open `RosPublisherExample.cs` and paste the following code: - - **Note** Script can be found at `tutorials/ros_unity_integration/unity_scripts` + - **Note** Script can be found at `tutorials/ros_unity_integration/unity_scripts` ```csharp using RosMessageTypes.RoboticsDemo; using UnityEngine; -using Random = UnityEngine.Random; +using Unity.Robotics.ROSTCPConnector; /// /// /// public class RosPublisherExample : MonoBehaviour { - public ROSConnection ros; + ROSConnection ros; public string topicName = "pos_rot"; - // The GameObject + // The game object public GameObject cube; // Publish the cube's position and rotation every N seconds public float publishMessageFrequency = 0.5f; // Used to determine how much time has elapsed since the last message was published private float timeElapsed; + + void Start() + { + // start the ROS connection + ros = ROSConnection.instance; + } private void Update() { @@ -82,14 +92,13 @@ public class RosPublisherExample : MonoBehaviour - Add a plane and a cube to the empty Unity scene - Move the cube a little ways up so it is hovering above the plane -- Create an empty GameObject, name it `RosConnection` and attach the `ROS TCP Connection/Runtime/TcpConnector/ROSConnection` script. - - Change the host name and port to match the ROS IP and port variables defined when you set up ROS +- In the main menu bar, open `Robotics/ROS Settings`. + - Set the ROS IP address and port to match the ROS IP and port variables defined when you set up ROS. - Create another empty GameObject, name it `RosPublisher` and attach the `RosPublisherExample` script. - - Drag the cube GameObject onto the `Cube` parameter - - Drag the RosConnection object onto its `Ros` parameter. + - Drag the cube GameObject onto the `Cube` parameter. - Pressing play in the Editor should publish a message to the terminal running the `rostopic echo pos_rot` command every 0.5 seconds ![](images/tcp_1.gif) -Continue to the [ROS Subscriber](subscriber.md) tutorial. +Continue to the [ROS Subscriber](subscriber.md) tutorial. \ No newline at end of file diff --git a/tutorials/ros_unity_integration/subscriber.md b/tutorials/ros_unity_integration/subscriber.md index 5ae578d6..70f5e250 100644 --- a/tutorials/ros_unity_integration/subscriber.md +++ b/tutorials/ros_unity_integration/subscriber.md @@ -11,7 +11,7 @@ Create a simple Unity scene which subscribes to a [ROS topic](http://wiki.ros.or - Follow the [ROS–Unity Initial Setup](setup.md) guide. - Open a new terminal window, navigate to your Catkin workspace, and run the following commands: - + ```bash source devel/setup.bash rosrun robotics_demo server_endpoint.py @@ -21,7 +21,7 @@ Once the server_endpoint has started, it will print something similar to `[INFO] - In Unity, we need to generate the C# code for the `UnityColor` message. Open `Robotics` -> `Generate ROS Messages...`. - Set the ROS message path to `PATH/TO/Unity-Robotics-Hub/tutorials/ros_packages/robotics_demo/`, expand the robotics_demo subfolder and click `Build 2 msgs`. - + ![](images/generate_messages_1.png) - The generated files will be saved in the default directory `Assets/RosMessages/RoboticsDemo/msg`. @@ -34,7 +34,7 @@ Once the server_endpoint has started, it will print something similar to `[INFO] ```csharp using UnityEngine; using Unity.Robotics.ROSTCPConnector; -using RosColor = RosMessageTypes.RoboticsDemo.MUnityColor; +using RosColor = RosMessageTypes.RoboticsDemo.UnityColor; public class RosSubscriberExample : MonoBehaviour { @@ -56,13 +56,16 @@ public class RosSubscriberExample : MonoBehaviour - Attach the `RosSubscriberExample` script to the `RosSubscriber` GameObject and drag the cube GameObject onto the `cube` parameter in the Inspector window. - From the Unity menu bar, open `Robotics/ROS Settings`, and set the `ROS IP Address` variable to your ROS IP. + - Unlike the previous tutorial, using a subscriber requires an incoming connection from ROS to Unity. You may need to adjust your firewall settings for this to work. + - The IP for Unity to listen on should be determined automatically, but if you're having trouble, you can set it manually in the `Override Unity IP` field. Finding the IP address of your local machine (the one running Unity) depends on your operating system. + - On a Mac, open `System Preferences > Network`. Your IP address should be listed on the active connection. + - On Windows, click the Wi-Fi icon on the taskbar, and open `Properties`. Your IP address should be listed near the bottom, next to "IPv4 address." + - Press play in the editor ### In ROS Terminal Window - After the scene has entered Play mode, run the following command: `rosrun robotics_demo color_publisher.py` to change the color of the cube GameObject in Unity to a random color -> Please reference [networking troubleshooting](network.md) doc if any errors are thrown. - ![](images/tcp_2.gif) Continue to the [ROS–Unity Integration Service](service.md). \ No newline at end of file From c78d4e99be5226f196a198551429b8856d886738 Mon Sep 17 00:00:00 2001 From: "Devin Miller (Unity)" Date: Tue, 16 Feb 2021 13:39:20 -0800 Subject: [PATCH 08/35] AIRO-405 Adding Yamato boilerplate to Robotics Hub (#144) * Adding testing boilerplate * Adding tests to run after message generation * Tweaks to ensure run works in Bokken image * Enabling audio by default, but disabling it in yamato config --- .gitignore | 1 + .../Assets/DemoScripts/Demo.cs | 6 +++--- .../Assets/DemoScripts/DemoScripts.asmdef | 20 +++++++++++++++++++ .../DemoScripts/DemoScripts.asmdef.meta | 7 +++++++ .../PickAndPlaceMessageGenerationtests.cs | 4 ++-- 5 files changed, 33 insertions(+), 5 deletions(-) create mode 100644 tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef create mode 100644 tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef.meta diff --git a/.gitignore b/.gitignore index 0382a3e3..ba15c0cb 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ .DS_Store +.swp .idea .vscode/ tutorials/pick_and_place/PickAndPlaceProject/Packages/packages-lock.json diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs index 99e9911c..8878ce80 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs +++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs @@ -1,14 +1,14 @@ #if UNITY_EDITOR using Microsoft.CSharp; -using RosSharp; -using RosSharp.Control; -using RosSharp.Urdf.Editor; using System; using System.CodeDom.Compiler; using System.Collections.Generic; using System.IO; using System.Reflection; using System.Text; +using RosSharp; +using RosSharp.Control; +using RosSharp.Urdf.Editor; using UnityEditor; using UnityEngine; using Unity.Robotics.ROSTCPConnector; diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef new file mode 100644 index 00000000..1e9e0b34 --- /dev/null +++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef @@ -0,0 +1,20 @@ +{ + "name": "DemoScripts", + "rootNamespace": "", + "references": [ + "GUID:625bfc588fb96c74696858f2c467e978", + "GUID:b1ef917f7a8a86a4eb639ec2352edbf8", + "GUID:465c1207fffb96245a352265e7622205" + ], + "includePlatforms": [ + "Editor" + ], + "excludePlatforms": [], + "allowUnsafeCode": false, + "overrideReferences": false, + "precompiledReferences": [], + "autoReferenced": true, + "defineConstraints": [], + "versionDefines": [], + "noEngineReferences": false +} \ No newline at end of file diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef.meta b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef.meta new file mode 100644 index 00000000..bea365c5 --- /dev/null +++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 80312ba658e2b5f41bf853abe51fbe2f +AssemblyDefinitionImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/PickAndPlaceMessageGenerationtests.cs b/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/PickAndPlaceMessageGenerationtests.cs index 09500a63..cfa57f8f 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/PickAndPlaceMessageGenerationtests.cs +++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/PickAndPlaceMessageGenerationtests.cs @@ -18,7 +18,7 @@ static IEnumerable IndividualMessages() { yield return new TestCaseData(Path.Combine(k_ROSDirectory, "moveit_msgs", "msg", "RobotTrajectory.msg")); } - + // Define directories of message files to be generated here static IEnumerable MessageDirectories() { @@ -46,7 +46,7 @@ public void TestMessageBuildDirectory_ThrowsNoExceptions(string directoryToBuild MessageAutoGen.GenerateDirectoryMessages(directoryToBuild, m_MessageGenOutputPath); AssetDatabase.Refresh(); } - + [Test] [TestCaseSource(nameof(ServiceDirectories))] public void TestServiceBuildDirectory_ThrowsNoExceptions(string directoryToBuild) From 3667cbe380bf57e8b1086c60f3279e7a10fa99b7 Mon Sep 17 00:00:00 2001 From: Devin Miller Date: Tue, 16 Feb 2021 14:31:11 -0800 Subject: [PATCH 09/35] Hotfix: Remove DemoScripts asmdef The DemoScripts.asmdef was a bit of experimental cruft that snuck in via my Yamato changes. This causes Demo.cs to compile as an Editor script, which means it can no longer be instantiated in the scene hierarchy (this breaks the demo). Removing the asmdef puts things back to normal. Tested by running the demo. --- .../Assets/DemoScripts/DemoScripts.asmdef | 20 ------------------- .../DemoScripts/DemoScripts.asmdef.meta | 7 ------- 2 files changed, 27 deletions(-) delete mode 100644 tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef delete mode 100644 tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef.meta diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef deleted file mode 100644 index 1e9e0b34..00000000 --- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef +++ /dev/null @@ -1,20 +0,0 @@ -{ - "name": "DemoScripts", - "rootNamespace": "", - "references": [ - "GUID:625bfc588fb96c74696858f2c467e978", - "GUID:b1ef917f7a8a86a4eb639ec2352edbf8", - "GUID:465c1207fffb96245a352265e7622205" - ], - "includePlatforms": [ - "Editor" - ], - "excludePlatforms": [], - "allowUnsafeCode": false, - "overrideReferences": false, - "precompiledReferences": [], - "autoReferenced": true, - "defineConstraints": [], - "versionDefines": [], - "noEngineReferences": false -} \ No newline at end of file diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef.meta b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef.meta deleted file mode 100644 index bea365c5..00000000 --- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/DemoScripts.asmdef.meta +++ /dev/null @@ -1,7 +0,0 @@ -fileFormatVersion: 2 -guid: 80312ba658e2b5f41bf853abe51fbe2f -AssemblyDefinitionImporter: - externalObjects: {} - userData: - assetBundleName: - assetBundleVariant: From 46a32f078af3b57058de071c1cc0ca46b36fa59d Mon Sep 17 00:00:00 2001 From: LaurieCheers <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Tue, 16 Feb 2021 10:59:59 -0800 Subject: [PATCH 10/35] Use latest main --- .../pick_and_place/PickAndPlaceProject/Packages/manifest.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json index ffb4a201..c2a1a3db 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json +++ b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json @@ -4,8 +4,8 @@ "com.unity.ide.rider": "2.0.7", "com.unity.ide.visualstudio": "2.0.3", "com.unity.ide.vscode": "1.2.2", - "com.unity.robotics.ros-tcp-connector": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#0b7ddbf36a13fbd47a0aeead062f0f57271b7e96", - "com.unity.robotics.urdf-importer": "https://github.com/Unity-Technologies/URDF-Importer.git#v0.1.2", + "com.unity.robotics.ros-tcp-connector": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#v0.2.0", + "com.unity.robotics.urdf-importer": "https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.2.0", "com.unity.test-framework": "1.1.18", "com.unity.textmeshpro": "3.0.1", "com.unity.timeline": "1.4.3", From 39f96a6fe2b93313f7b8463841a865a9d9c0b237 Mon Sep 17 00:00:00 2001 From: Devin Miller Date: Fri, 19 Feb 2021 17:44:23 -0800 Subject: [PATCH 11/35] Fixing issue with Demo.cs not working with new URDF Importer co-routine --- .../Assets/DemoScripts/Demo.cs | 74 ++++++++++--------- 1 file changed, 39 insertions(+), 35 deletions(-) diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs index 8878ce80..348cb0c6 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs +++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs @@ -9,6 +9,7 @@ using RosSharp; using RosSharp.Control; using RosSharp.Urdf.Editor; +using Unity.EditorCoroutines.Editor; using UnityEditor; using UnityEngine; using Unity.Robotics.ROSTCPConnector; @@ -75,8 +76,10 @@ void Awake() { EditorApplication.LockReloadAssemblies(); SetupScene(); - SetupRos(); - CreateTrajectoryPlannerPubliser(); + AddRosMessages(); + ImportRobot(); + CreateRosConnection(); + CreateTrajectoryPlannerPublisher(); } void Update() @@ -104,31 +107,18 @@ void OnApplicationQuit() private void SetupScene() { // Instantiate the table, target, and target placement - GameObject table = InstantiatePrefab(tableName); - GameObject target = InstantiatePrefab(targetName); - GameObject targetPlacement = InstantiatePrefab(targetPlacementName); + InstantiatePrefab(tableName); + InstantiatePrefab(targetName); + InstantiatePrefab(targetPlacementName); // Adjust main camera GameObject camera = GameObject.Find(cameraName); camera.transform.position = cameraPosition; camera.transform.rotation = cameraRotation; - - // Import Niryo One with URDF importer - string urdfFilepath = Path.Combine(Application.dataPath, urdfRelativeFilepath); - ImportRobot(urdfFilepath, ImportSettings.convexDecomposer.unity); - - // Adjust robot parameters - Controller controller = GameObject.Find(niryoOneName).GetComponent(); - controller.stiffness = controllerStiffness; - controller.damping = controllerDamping; - controller.forceLimit = controllerForceLimit; - controller.speed = controllerSpeed; - controller.acceleration = controllerAcceleration; - GameObject.Find(baseLinkName).GetComponent().immovable = true; } // Tutorial Part 2 without the Publisher object - private void SetupRos() + private void AddRosMessages() { if (generateRosMessages) { @@ -152,19 +142,9 @@ private void SetupRos() scripts.AddRange(Directory.GetFiles(rosMessagesDirectory, scriptPattern, SearchOption.AllDirectories)); scripts.AddRange(Directory.GetFiles(externalScriptsDirectory)); RecompileScripts(scripts.ToArray()); - - // Create RosConnect - GameObject rosConnect = new GameObject(rosConnectName); - rosConnection = rosConnect.AddComponent(); - rosConnection.rosIPAddress = hostIP; - rosConnection.rosPort = hostPort; - rosConnection.overrideUnityIP = overrideUnityIP; - rosConnection.unityPort = unityPort; - rosConnection.awaitDataMaxRetries = awaitDataMaxRetries; - rosConnection.awaitDataSleepSeconds = awaitDataSleepSeconds; } - private void CreateTrajectoryPlannerPubliser() + private void CreateTrajectoryPlannerPublisher() { GameObject publisher = new GameObject(publisherName); dynamic planner = publisher.AddComponent(assembly.GetType(trajectoryPlannerType)); @@ -182,15 +162,39 @@ private GameObject InstantiatePrefab(string name) return gameObject; } - private void ImportRobot(string urdfFilepath, ImportSettings.convexDecomposer convexDecomposer) + private void CreateRosConnection() { - // Import Niryo One by URDF Importer - ImportSettings settings = new ImportSettings + // Create RosConnect + GameObject rosConnect = new GameObject(rosConnectName); + rosConnection = rosConnect.AddComponent(); + rosConnection.rosIPAddress = hostIP; + rosConnection.rosPort = hostPort; + rosConnection.overrideUnityIP = overrideUnityIP; + rosConnection.unityPort = unityPort; + rosConnection.awaitDataMaxRetries = awaitDataMaxRetries; + rosConnection.awaitDataSleepSeconds = awaitDataSleepSeconds; + } + + private void ImportRobot() + { + ImportSettings urdfImportSettings = new ImportSettings { choosenAxis = ImportSettings.axisType.yAxis, - convexMethod = convexDecomposer, + convexMethod = ImportSettings.convexDecomposer.unity }; - UrdfRobotExtensions.Create(urdfFilepath, settings); + // Import Niryo One with URDF importer + string urdfFilepath = Path.Combine(Application.dataPath, urdfRelativeFilepath); + // Create is a coroutine that would usually run only in EditMode, so we need to force its execution here + var robotImporter = UrdfRobotExtensions.Create(urdfFilepath, urdfImportSettings, false); + while (robotImporter.MoveNext()) {} + // Adjust robot parameters + Controller controller = GameObject.Find(niryoOneName).GetComponent(); + controller.stiffness = controllerStiffness; + controller.damping = controllerDamping; + controller.forceLimit = controllerForceLimit; + controller.speed = controllerSpeed; + controller.acceleration = controllerAcceleration; + GameObject.Find(baseLinkName).GetComponent().immovable = true; } // Credit to https://www.codeproject.com/Tips/715891/Compiling-Csharp-Code-at-Runtime From f3b99980d8d10454dd8f7c56ac32e9bd3dde88d6 Mon Sep 17 00:00:00 2001 From: peifeng-unity <56408141+peifeng-unity@users.noreply.github.com> Date: Tue, 23 Feb 2021 00:25:34 +0000 Subject: [PATCH 12/35] remove unused import (#163) --- .../PickAndPlaceProject/Assets/DemoScripts/Demo.cs | 1 - 1 file changed, 1 deletion(-) diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs index 348cb0c6..0b4a591b 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs +++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs @@ -9,7 +9,6 @@ using RosSharp; using RosSharp.Control; using RosSharp.Urdf.Editor; -using Unity.EditorCoroutines.Editor; using UnityEditor; using UnityEngine; using Unity.Robotics.ROSTCPConnector; From 9919a2974d7b9a93fb5120b7dd1f5bc5fb43afe9 Mon Sep 17 00:00:00 2001 From: peifeng-unity <56408141+peifeng-unity@users.noreply.github.com> Date: Wed, 24 Feb 2021 23:47:57 +0000 Subject: [PATCH 13/35] update package import urls for tcp connector and urdf importer (#166) --- tutorials/quick_setup.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/tutorials/quick_setup.md b/tutorials/quick_setup.md index 2c714958..c240031c 100644 --- a/tutorials/quick_setup.md +++ b/tutorials/quick_setup.md @@ -2,19 +2,19 @@ This page provides brief instructions on installing the Unity Robotics packages. Head over to the [Pick-and-Place Tutorial](pick_and_place/README.md) for more detailed instructions and steps for building a sample project. -1. Create or open a Unity project. +1. Create or open a Unity project. > Note: If you are adding the URDF-Importer, ensure you are using a [2020.2.0](https://unity3d.com/unity/whats-new/2020.2.0)+ version of Unity Editor. 1. Open `Window` -> `Package Manager`. -1. In the Package Manager window, find and click the `+` button in the upper lefthand corner of the window. Select `Add package from git URL...`. +1. In the Package Manager window, find and click the `+` button in the upper lefthand corner of the window. Select `Add package from git URL...`. ![](../images/packman.png) -1. Enter the git URL for the desired package. Note: you can append a version tag to the end of the git url, like `#v0.2.0` or `#v0.3.0`, to declare a specific package version, or exclude the tag to get the latest from the package's `main` branch. - 1. For the [ROS-TCP-Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector), enter `https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector`. - 1. For the [URDF-Importer](https://github.com/Unity-Technologies/URDF-Importer), enter `https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer`. +1. Enter the git URL for the desired package. + 1. For the [ROS-TCP-Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector), enter `https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#v0.2.0`. + 1. For the [URDF-Importer](https://github.com/Unity-Technologies/URDF-Importer), enter `https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.2.0`. -1. Click `Add`. +1. Click `Add`. To install from a local clone of the repository, see [installing a local package](https://docs.unity3d.com/Manual/upm-ui-local.html) in the Unity manual. From 8a6e2131f308e6be992f2537291c3b978c927170 Mon Sep 17 00:00:00 2001 From: vidurvij-Unity <60901103+vidurvij-Unity@users.noreply.github.com> Date: Thu, 25 Feb 2021 10:01:36 -0800 Subject: [PATCH 14/35] Update urdf_tutorial.md (#168) --- tutorials/urdf_importer/urdf_tutorial.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/tutorials/urdf_importer/urdf_tutorial.md b/tutorials/urdf_importer/urdf_tutorial.md index e4935c89..966610f5 100644 --- a/tutorials/urdf_importer/urdf_tutorial.md +++ b/tutorials/urdf_importer/urdf_tutorial.md @@ -6,20 +6,20 @@ - Niryo One URDF files from [Niryo One ROS](https://github.com/NiryoRobotics/niryo_one_ros) - Working ROS environment -## Setting up the URDF Importer in Unity Editor +## Setting up the URDF Importer in Unity Editor - Integrate the URDF Importer following these [instructions](https://github.com/Unity-Technologies/URDF-Importer#integrate-urdf-importer-into-unity-project) - Create a new directory in `Assets` and name it `URDF` - Clone the [Niryo One ROS](https://github.com/NiryoRobotics/niryo_one_ros) repo and copy the `niryo_one_description` directory into `Assets/URDF` - This directory only includes a `.urdf.xacro` file which will need to be converted into a `.urdf` file before we can import it - Run the following command to convert Xacro to URDF, `rosrun xacro xacro --inorder -o PATH/TO/niryo_one.urdf PATH/TO/niryo_one.urdf.xacro` - Copy the generated `niryo_one.urdf` file to `Assets/URDF` - - Right click on the this file and select `Import Robot from URDF` - - Select the co-ordinate system in which the meshes were designed. Default mesh orientation is Y-up which is supported by Unity but some packages often use Z-up and X-up configuration. For more [information](https://docs.unity3d.com/Manual/HOWTO-FixZAxisIsUp.html). + - Right click on the this file and select `Import Robot from URDF` + - Select the co-ordinate system in which the meshes were designed. Default mesh orientation is Y-up which is supported by Unity but some packages often use Z-up and X-up configuration. For more [information](https://docs.unity3d.com/Manual/HOWTO-FixZAxisIsUp.html). - Select the Convex Mesh Decomposer you want to use for the imported robot. More information can be found [here](urdf_appendix.md##Convex-Mesh-Collider). - Click `Import` - + ## Using the Controller -- To add the controller to an imported robot click the `Enable` button in the Inspector window in front of the `Controller Script` option. This will add a Controller Script, FKrobot and Joint Control at runtime. +- To add the controller to an imported robot click the `Enable` button in the Inspector window in front of the `Controller Script` option. This will add a Controller Script, FKrobot and Joint Control at runtime. - To prevent the joints from slipping set the `Stiffness` and `Damping` to `100,000` and `10,000` respectively. - To be able to apply forces to the joints set the `Force Limit` to `10,000`. - To prevent the robot from falling over, in the GameObject tree expand `niryo_one` -> `world` -> `base_link` and set the toggle for `Immovable` for the base_link. From 042de75d363d5e09a917901574d10ff818ed59ca Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Fri, 26 Feb 2021 10:19:08 -0800 Subject: [PATCH 15/35] Added M prefixes to docs (#167) --- tutorials/pick_and_place/2_ros_tcp.md | 62 +++++++++---------- .../Scripts/SourceDestinationPublisher.cs | 7 ++- .../Scripts/TrajectoryPlanner.cs | 18 +++--- .../Scripts_Part4/RealSimPickAndPlace.cs | 18 +++--- tutorials/ros_unity_integration/publisher.md | 2 +- tutorials/ros_unity_integration/subscriber.md | 2 +- .../unity_scripts/RosServiceExample.cs | 4 +- .../unity_scripts/RosSubscriberExample.cs | 2 +- 8 files changed, 58 insertions(+), 57 deletions(-) diff --git a/tutorials/pick_and_place/2_ros_tcp.md b/tutorials/pick_and_place/2_ros_tcp.md index 033c5169..dbec6059 100644 --- a/tutorials/pick_and_place/2_ros_tcp.md +++ b/tutorials/pick_and_place/2_ros_tcp.md @@ -25,7 +25,7 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n ## The Unity Side -1. If you have not already completed the steps in [Part 0](0_ros_setup.md) to set up your ROS workspace and [Part 1](1_urdf.md) to set up the Unity project, do so now. +1. If you have not already completed the steps in [Part 0](0_ros_setup.md) to set up your ROS workspace and [Part 1](1_urdf.md) to set up the Unity project, do so now. 1. If the PickAndPlaceProject Unity project is not already open, select and open it from the Unity Hub. @@ -45,36 +45,36 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n ![](img/2_menu.png) - In the ROS Message Browser window, click `Browse` next to the ROS message path. Navigate to and select the ROS directory of this cloned repository (`Unity-Robotics-Hub/tutorials/pick_and_place/ROS/`). This window will populate with all msg and srv files found in this directory. - + In the ROS Message Browser window, click `Browse` next to the ROS message path. Navigate to and select the ROS directory of this cloned repository (`Unity-Robotics-Hub/tutorials/pick_and_place/ROS/`). This window will populate with all msg and srv files found in this directory. + ![](img/2_browser.png) > Note: If any of these ROS directories appear to be empty, you can run the command `git submodule update --init --recursive` to download the packages via Git submodules. - + Under `ROS/src/moveit_msgs/msg`, scroll to `RobotTrajectory.msg`, and click its `Build msg` button. The button text will change to "Rebuild msg" when it has finished building. ![](img/2_robottraj.png) - One new C# script should populate the `Assets/RosMessages/Moveit/msg` directory: MRobotTrajectory. This name is the same as the message you built, with an "M" prefix (for message). - -1. Next, the custom message scripts for this tutorial will need to be generated. + +1. Next, the custom message scripts for this tutorial will need to be generated. Still in the ROS Message Browser window, expand `ROS/src/niryo_moveit/msg` to view the msg files listed. Next to msg, click `Build 2 msgs`. ![](img/2_msg.png) - + - Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/msg` directory: MNiryoMoveitJoints and MNiryoTrajectory. MNiryoMoveitJoints describes a value for each joint in the Niryo arm as well as poses for the target object and target goal. MNiryoTrajectory describes a list of RobotTrajectory values, which will hold the calculated trajectories for the pick-and-place task. - + > MessageGeneration generates a C# class from a ROS msg file with protections for use of C# reserved keywords and conversion to C# datatypes. Learn more about [ROS Messages](https://wiki.ros.org/Messages). -1. Finally, now that the messages have been generated, we will create the service for moving the robot. +1. Finally, now that the messages have been generated, we will create the service for moving the robot. - Still in the ROS Message Browser window, expand `ROS/src/niryo_moveit/srv` to view the srv file listed. Next to srv, click `Build 1 srv`. + Still in the ROS Message Browser window, expand `ROS/src/niryo_moveit/srv` to view the srv file listed. Next to srv, click `Build 1 srv`. ![](img/2_srv.png) - - Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/srv` directory: MMoverServiceRequest and MMoverServiceResponse. These files describe the expected input and output formats for the service requests and responses when calculating trajectories. - + - Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/srv` directory: MMoverServiceRequest and MMoverServiceResponse. These files describe the expected input and output formats for the service requests and responses when calculating trajectories. + > MessageGeneration generates two C# classes, a request and response, from a ROS srv file with protections for use of C# reserved keywords and conversion to C# datatypes. Learn more about [ROS Services](https://wiki.ros.org/Services). You can now close the ROS Message Browser window. @@ -115,24 +115,24 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n } ``` - > This function first takes in the current joint target values. Then, it grabs the poses of the `target` and the `targetPlacement` objects, adds them to the newly created message `sourceDestinationMessage`, and calls `Send()` to send this information to the ROS topic `topicName` (defined as `"SourceDestination_input"`). + > This function first takes in the current joint target values. Then, it grabs the poses of the `target` and the `targetPlacement` objects, adds them to the newly created message `sourceDestinationMessage`, and calls `Send()` to send this information to the ROS topic `topicName` (defined as `"SourceDestination_input"`). > Note: Going from Unity world space to ROS world space requires a conversion. Unity's `(x,y,z)` is equivalent to the ROS `(z,-x,y)` coordinate. These conversions are provided via the [ROSGeometry component](https://github.com/Unity-Technologies/ROS-TCP-Connector/blob/main/ROSGeometry.md) in the ROS-TCP-Connector package. -1. Return to the Unity Editor. Now that the message contents have been defined and the publisher script added, it needs to be added to the Unity world to run its functionality. +1. Return to the Unity Editor. Now that the message contents have been defined and the publisher script added, it needs to be added to the Unity world to run its functionality. - Right click in the Hierarchy window and select "Create Empty" to add a new empty GameObject. Name it `Publisher`. Add the newly created SourceDestinationPublisher component to the Publisher GameObject by selecting the Publisher object. Click "Add Component" in the Inspector, and begin typing "SourceDestinationPublisher." Select the component when it appears. + Right click in the Hierarchy window and select "Create Empty" to add a new empty GameObject. Name it `Publisher`. Add the newly created SourceDestinationPublisher component to the Publisher GameObject by selecting the Publisher object. Click "Add Component" in the Inspector, and begin typing "SourceDestinationPublisher." Select the component when it appears. > Note: Alternatively, you can drag the script from the Project window onto the Publisher object in the Hierarchy window. ![](img/2_sourcedest.gif) -1. Note that this component shows empty member variables in the Inspector window, which need to be assigned. +1. Note that this component shows empty member variables in the Inspector window, which need to be assigned. Select the Target object in the Hierarchy and assign it to the `Target` field in the Publisher. Similarly, assign the TargetPlacement object to the `TargetPlacement` field. Assign the niryo_one robot to the `Niryo One` field. ![](img/2_target.gif) -1. Next, the ROS TCP connection needs to be created. Select `Robotics -> ROS Settings` from the top menu bar. +1. Next, the ROS TCP connection needs to be created. Select `Robotics -> ROS Settings` from the top menu bar. In the ROS Settings window, the `ROS IP Address` should be the IP address of your ROS machine (*not* the one running Unity). @@ -140,19 +140,19 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n - If you are **not** running ROS services in a Docker container, replace the `ROS IP Address` value with the IP address of your ROS machine. Ensure that the `Host Port` is set to `10000`. - - If you **are** running ROS services in a Docker container, fill `ROS IP Address` with the loopback IP address `127.0.0.1` and the `Override Unity IP Address` as your local machine's IP address. Otherwise, leave the `Override Unity IP Address` field empty. + - If you **are** running ROS services in a Docker container, fill `ROS IP Address` and `Override Unity IP Address` with the loopback IP address `127.0.0.1`. Otherwise, leave the `Override Unity IP Address` field empty. ![](img/2_settings.png) Opening the ROS Settings has created a ROSConnectionPrefab in `Assets/Resources` with the user-input settings. When the static `ROSConnection.instance` is referenced in a script, if a `ROSConnection` instance is not already present, the prefab will be instantiated in the Unity scene, and the connection will begin. - + > Note: While using the ROS Settings menu is the suggested workflow as of this version, you may still manually create a GameObject with an attached ROSConnection component. -1. Next, we will add a UI element that will allow user input to trigger the `Publish()` function. In the Hierarchy window, right click to add a new UI > Button. Note that this will create a new Canvas parent as well. +1. Next, we will add a UI element that will allow user input to trigger the `Publish()` function. In the Hierarchy window, right click to add a new UI > Button. Note that this will create a new Canvas parent as well. > Note: In the `Game` view, you will see the button appear in the bottom left corner as an overlay. In `Scene` view the button will be rendered on a canvas object that may not be visible. - - > Note: In case the Button does not start in the bottom left, it can be moved by setting the `Pos X` and `Pos Y` values in its Rect Transform component. For example, setting its Position to `(-200, -200, 0)` would set its position to the bottom right area of the screen. - + + > Note: In case the Button does not start in the bottom left, it can be moved by setting the `Pos X` and `Pos Y` values in its Rect Transform component. For example, setting its Position to `(-200, -200, 0)` would set its position to the bottom right area of the screen. + 1. Select the newly made Button object, and scroll to see the Button component in the Inspector. Click the `+` button under the empty `OnClick()` header to add a new event. Select the `Publisher` object in the Hierarchy window and drag it into the new OnClick() event, where it says `None (Object)`. Click the dropdown where it says `No Function`. Select SourceDestinationPublisher > `Publish()`. ![](img/2_onclick.png) @@ -175,7 +175,7 @@ Most of the ROS setup has been provided via the `niryo_moveit` package. This sec roslaunch niryo_moveit part_2.launch ``` - > Note: Running `roslaunch` automatically starts [ROS Core](http://wiki.ros.org/roscore) if it is not already running. + > Note: Running `roslaunch` automatically starts [ROS Core](http://wiki.ros.org/roscore) if it is not already running. > Note: This launch file has been copied below for reference. The server_endpoint and trajectory_subscriber nodes are launched from this file, and the ROS params (set up in [Part 0](0_ros_setup.md)) are loaded from this command. The launch files for this project are available in the package's `launch` directory, i.e. `src/niryo_moveit/launch/`. @@ -187,12 +187,12 @@ Most of the ROS setup has been provided via the `niryo_moveit` package. This sec ``` - This launch will print various messages to the console, including the set parameters and the nodes launched. - + This launch will print various messages to the console, including the set parameters and the nodes launched. + Ensure that the `process[server_endpoint]` and `process[trajectory_subscriber]` were successfully started, and that a message similar to `[INFO] [1603488341.950794]: Starting server on 192.168.50.149:10000` is printed. 1. Return to Unity, and press Play. Click the UI Button in the Game view to call SourceDestinationPublisher's `Publish()` function, publishing the associated data to the ROS topic. View the terminal in which the `roslaunch` command is running. It should now print `I heard:` with the data. - + ROS and Unity have now successfully connected! ![](img/2_echo.png) @@ -203,8 +203,8 @@ ROS and Unity have now successfully connected! - If the error `[rosrun] Found the following, but they're either not files, or not executable: server_endpoint.py` appears, the Python script may need to be marked as executable via `chmod +x Unity-Robotics-Hub/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py`. - `...failed because unknown error handler name 'rosmsg'` This is due to a bug in an outdated package version. Try running `sudo apt-get update && sudo apt-get upgrade` to upgrade. - -- If Unity fails to find a network connection, ensure that the ROS IP address is entered correctly as the ROS IP Address in the RosConnect in Unity, and that the `src/niryo_moveit/config/params.yaml` values are set correctly. + +- If Unity fails to find a network connection, ensure that the ROS IP address is entered correctly as the ROS IP Address in the RosConnect in Unity, and that the `src/niryo_moveit/config/params.yaml` values are set correctly. - If the ROS TCP handshake fails (e.g. `ROS-Unity server listening...` printed on the Unity side but no `ROS-Unity Handshake received` on the ROS side), the ROS IP may not have been set correctly in the params.yaml file. Try running `echo "ROS_IP: $(hostname -I)" > src/niryo_moveit/config/params.yaml` in a terminal from your ROS workspace. @@ -218,7 +218,7 @@ ROS and Unity have now successfully connected! - [TCP Endpoint](https://github.com/Unity-Technologies/ROS-TCP-Endpoint) package - [Niryo One ROS stack](https://github.com/NiryoRobotics/niryo_one_ros) - [MoveIt Msgs](https://github.com/ros-planning/moveit_msgs) - + --- -#### Proceed to [Part 3](3_pick_and_place.md). \ No newline at end of file +#### Proceed to [Part 3](3_pick_and_place.md). diff --git a/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs b/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs index f0aa675c..c5e0a6b2 100644 --- a/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs +++ b/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs @@ -2,6 +2,7 @@ using UnityEngine; using Unity.Robotics.ROSTCPConnector; using Unity.Robotics.ROSTCPConnector.ROSGeometry; +using RosMessageTypes.Geometry; public class SourceDestinationPublisher : MonoBehaviour { @@ -51,7 +52,7 @@ void Start() public void Publish() { - NiryoMoveitJoints sourceDestinationMessage = new NiryoMoveitJoints(); + MNiryoMoveitJoints sourceDestinationMessage = new MNiryoMoveitJoints(); sourceDestinationMessage.joint_00 = jointArticulationBodies[0].xDrive.target; sourceDestinationMessage.joint_01 = jointArticulationBodies[1].xDrive.target; @@ -61,14 +62,14 @@ public void Publish() sourceDestinationMessage.joint_05 = jointArticulationBodies[5].xDrive.target; // Pick Pose - sourceDestinationMessage.pick_pose = new RosMessageTypes.Geometry.Pose + sourceDestinationMessage.pick_pose = new MPose { position = target.transform.position.To(), orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To() }; // Place Pose - sourceDestinationMessage.place_pose = new RosMessageTypes.Geometry.Pose + sourceDestinationMessage.place_pose = new MPose { position = targetPlacement.transform.position.To(), orientation = pickOrientation.To() diff --git a/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs b/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs index f14015a1..ead3e41b 100644 --- a/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs +++ b/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs @@ -4,7 +4,7 @@ using UnityEngine; using Unity.Robotics.ROSTCPConnector; using Unity.Robotics.ROSTCPConnector.ROSGeometry; - +using RosMessageTypes.Geometry; public class TrajectoryPlanner : MonoBehaviour { @@ -78,9 +78,9 @@ private void OpenGripper() /// Get the current values of the robot's joint angles. /// /// NiryoMoveitJoints - NiryoMoveitJoints CurrentJointConfig() + MNiryoMoveitJoints CurrentJointConfig() { - NiryoMoveitJoints joints = new NiryoMoveitJoints(); + MNiryoMoveitJoints joints = new MNiryoMoveitJoints(); joints.joint_00 = jointArticulationBodies[0].xDrive.target; joints.joint_01 = jointArticulationBodies[1].xDrive.target; @@ -101,11 +101,11 @@ NiryoMoveitJoints CurrentJointConfig() /// public void PublishJoints() { - MoverServiceRequest request = new MoverServiceRequest(); + MMoverServiceRequest request = new MMoverServiceRequest(); request.joints_input = CurrentJointConfig(); // Pick Pose - request.pick_pose = new RosMessageTypes.Geometry.Pose + request.pick_pose = new MPose { position = (target.transform.position + pickPoseOffset).To(), // The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping. @@ -113,16 +113,16 @@ public void PublishJoints() }; // Place Pose - request.place_pose = new RosMessageTypes.Geometry.Pose + request.place_pose = new MPose { position = (targetPlacement.transform.position + pickPoseOffset).To(), orientation = pickOrientation.To() }; - ros.SendServiceMessage(rosServiceName, request, TrajectoryResponse); + ros.SendServiceMessage(rosServiceName, request, TrajectoryResponse); } - void TrajectoryResponse(MoverServiceResponse response) + void TrajectoryResponse(MMoverServiceResponse response) { if (response.trajectories.Length > 0) { @@ -149,7 +149,7 @@ void TrajectoryResponse(MoverServiceResponse response) /// /// MoverServiceResponse received from niryo_moveit mover service running in ROS /// - private IEnumerator ExecuteTrajectories(MoverServiceResponse response) + private IEnumerator ExecuteTrajectories(MMoverServiceResponse response) { if (response.trajectories != null) { diff --git a/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs b/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs index 3e9a32f8..724f710d 100644 --- a/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs +++ b/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs @@ -1,12 +1,12 @@ using System.Collections; using System.Linq; -using ROSGeometry; +using RosMessageTypes.Geometry; using RosMessageTypes.Moveit; using RosMessageTypes.NiryoMoveit; using RosMessageTypes.NiryoOne; +using Unity.Robotics.ROSTCPConnector; +using Unity.Robotics.ROSTCPConnector.ROSGeometry; using UnityEngine; -using Pose = RosMessageTypes.Geometry.Pose; -using RosQuaternion = RosMessageTypes.Geometry.Quaternion; public class RealSimPickAndPlace : MonoBehaviour { @@ -81,7 +81,7 @@ private void OpenGripper() /// public void PublishJoints() { - MoverServiceRequest request = new MoverServiceRequest + MMoverServiceRequest request = new MMoverServiceRequest { joints_input = { @@ -92,13 +92,13 @@ public void PublishJoints() joint_04 = jointArticulationBodies[4].xDrive.target, joint_05 = jointArticulationBodies[5].xDrive.target }, - pick_pose = new Pose + pick_pose = new MPose { position = (target.transform.position + PICK_POSE_OFFSET).To(), // The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping. orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To() }, - place_pose = new Pose + place_pose = new MPose { position = (targetPlacement.transform.position + PICK_POSE_OFFSET).To(), orientation = pickOrientation.To() @@ -146,7 +146,7 @@ void Awake() void Start() { - ros.Subscribe(rosRobotCommandsTopicName, ExecuteRobotCommands); + ros.Subscribe(rosRobotCommandsTopicName, ExecuteRobotCommands); } /// @@ -155,7 +155,7 @@ void Start() /// executed in a coroutine. /// /// RobotMoveActionGoal of trajectory or gripper commands - void ExecuteRobotCommands(RobotMoveActionGoal robotAction) + void ExecuteRobotCommands(MRobotMoveActionGoal robotAction) { if (robotAction.goal.cmd.cmd_type == TRAJECTORY_COMMAND_EXECUTION) { @@ -184,7 +184,7 @@ void ExecuteRobotCommands(RobotMoveActionGoal robotAction) /// /// /// The array of poses for the robot to execute - private IEnumerator ExecuteTrajectories(RobotTrajectory trajectories) + private IEnumerator ExecuteTrajectories(MRobotTrajectory trajectories) { // For every robot pose in trajectory plan foreach (var point in trajectories.joint_trajectory.points) diff --git a/tutorials/ros_unity_integration/publisher.md b/tutorials/ros_unity_integration/publisher.md index b50b3e47..f736af41 100644 --- a/tutorials/ros_unity_integration/publisher.md +++ b/tutorials/ros_unity_integration/publisher.md @@ -71,7 +71,7 @@ public class RosPublisherExample : MonoBehaviour { cube.transform.rotation = Random.rotation; - PosRot cubePos = new PosRot( + MPosRot cubePos = new MPosRot( cube.transform.position.x, cube.transform.position.y, cube.transform.position.z, diff --git a/tutorials/ros_unity_integration/subscriber.md b/tutorials/ros_unity_integration/subscriber.md index 70f5e250..ac5164e9 100644 --- a/tutorials/ros_unity_integration/subscriber.md +++ b/tutorials/ros_unity_integration/subscriber.md @@ -34,7 +34,7 @@ Once the server_endpoint has started, it will print something similar to `[INFO] ```csharp using UnityEngine; using Unity.Robotics.ROSTCPConnector; -using RosColor = RosMessageTypes.RoboticsDemo.UnityColor; +using RosColor = RosMessageTypes.RoboticsDemo.MUnityColor; public class RosSubscriberExample : MonoBehaviour { diff --git a/tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs b/tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs index d2e419b8..3a715a0b 100644 --- a/tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs +++ b/tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs @@ -47,7 +47,7 @@ private void Update() // Send message to ROS and return the response ros.SendServiceMessage(serviceName, positionServiceRequest, Callback_Destination); - awaitingResponseUntilTimestamp = Time.time + 1.0f; // don't send again for 1 second, or until we receive a response + awaitingResponseUntilTimestamp = Time.time+1.0f; // don't send again for 1 second, or until we receive a response } } @@ -57,4 +57,4 @@ void Callback_Destination(MPositionServiceResponse response) destination = new Vector3(response.output.pos_x, response.output.pos_y, response.output.pos_z); Debug.Log("New Destination: " + destination); } -} +} \ No newline at end of file diff --git a/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs b/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs index 7040b625..6b3a69a6 100644 --- a/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs +++ b/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs @@ -1,6 +1,6 @@ using UnityEngine; using Unity.Robotics.ROSTCPConnector; -using RosColor = RosMessageTypes.RoboticsDemo.UnityColor; +using RosColor = RosMessageTypes.RoboticsDemo.MUnityColor; public class RosSubscriberExample : MonoBehaviour { From 870efce4cb74ea377543ea65399bd46d74284523 Mon Sep 17 00:00:00 2001 From: "Devin Miller (Unity)" Date: Wed, 10 Mar 2021 12:12:19 -0800 Subject: [PATCH 16/35] Adding Editor version to Ros Integration Readme --- tutorials/ros_unity_integration/README.md | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/tutorials/ros_unity_integration/README.md b/tutorials/ros_unity_integration/README.md index 45997364..8ed68633 100644 --- a/tutorials/ros_unity_integration/README.md +++ b/tutorials/ros_unity_integration/README.md @@ -14,12 +14,9 @@ The `ROSConnection` plugin (also from [ROS TCP Connector](https://github.com/Uni ## Tutorials -- [ROS–Unity Integration: Initial Setup](setup.md) - ROS-Unity Initial Setup -- [ROS–Unity Integration: Network Description](network.md) - Description of network settings and troubleshooting - [ROS–Unity Integration: Publisher](publisher.md) - Adding a Publisher to a Unity Scene - [ROS–Unity Integration: Subscriber](subscriber.md) - Adding a Subscriber to a Unity Scene - [ROS–Unity Integration: Service](service.md) - Adding a Service call to a Unity Scene -- [ROS–Unity Integration: UnityService](unity_service.md) - Adding a Service that runs in a Unity Scene - [ROS–Unity Integration: Server Endpoint](server_endpoint.md) - How to write a Server Endpoint ## Example Unity Scripts @@ -29,12 +26,8 @@ Example scripts implemented in tutorials: - `unity_scripts/RosPublisherExample.cs` - Publishes the position of a GameObject every 0.5 seconds. -- `unity_scripts/RosSubscriberExample.cs` - - Subscribes to a topic that accepts color messages and uses them to change the color of a GameObject in the Unity scene. - - `unity_scripts/RosServiceExample.cs` - Returns a destination position for a GameObject to move towards each time the service is called. -- `unity_scripts/RosUnityServiceExample.cs` - - Runs a service in the Unity scene that takes a GameObject's name and responds with the Pose of that object. - +- `unity_scripts/RosSubscriberExample.cs` + - Subscribes to a topic that accepts color messages and uses them to change the color of a GameObject in the Unity scene. From 79afbe935a63bfff32ddac31bca56a8c2f30adfa Mon Sep 17 00:00:00 2001 From: Devin Miller Date: Wed, 17 Mar 2021 12:55:59 -0700 Subject: [PATCH 17/35] Updating package dependencies to proper versions Pick and place tutorial was failing to compile its scripts because the package manifest was pointing to an older version of ROS-TCP-Connector. Updated the manifest and also pulled latest changes for ros_tcp_endpoint. Tested by running DemoScene --- .../pick_and_place/PickAndPlaceProject/Packages/manifest.json | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json index c2a1a3db..812be142 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json +++ b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json @@ -4,7 +4,8 @@ "com.unity.ide.rider": "2.0.7", "com.unity.ide.visualstudio": "2.0.3", "com.unity.ide.vscode": "1.2.2", - "com.unity.robotics.ros-tcp-connector": "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#v0.2.0", + "com.unity.robotics.ros-tcp-connector": + "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#dev", "com.unity.robotics.urdf-importer": "https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.2.0", "com.unity.test-framework": "1.1.18", "com.unity.textmeshpro": "3.0.1", From 461645244854601ed97a525b844b48af22a46734 Mon Sep 17 00:00:00 2001 From: Amanda <31416491+at669@users.noreply.github.com> Date: Mon, 29 Mar 2021 17:05:31 -0600 Subject: [PATCH 18/35] Updating Contribution Content (#186) * Contributing content * PR feedback --- CONTRIBUTING.md | 16 ++++++++-------- README.md | 14 ++++++++++++-- 2 files changed, 20 insertions(+), 10 deletions(-) diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 18edb9f8..7cc07605 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,7 +1,7 @@ # Contribution Guidelines -Thank you for your interest in contributing to Unity Robotics! To facilitate your -contributions, we've outlined a brief set of guidelines to ensure that your extensions +Thank you for your interest in contributing to Unity Robotics! To facilitate your +contributions, we've outlined a brief set of guidelines to ensure that your extensions can be easily integrated. ## Communication @@ -40,10 +40,10 @@ We run continuous integration on all PRs; all tests must be passing before the P All Python code should follow the [PEP 8 style guidelines](https://pep8.org/). -All C# code should follow the [Microsoft C# Coding Conventions](https://docs.microsoft.com/en-us/dotnet/csharp/programming-guide/inside-a-program/coding-conventions). -Additionally, the [Unity Coding package](https://docs.unity3d.com/Packages/com.unity.coding@0.1/manual/index.html) -can be used to format, encode, and lint your code according to the standard Unity -development conventions. Be aware that these Unity conventions will supersede the +All C# code should follow the [Microsoft C# Coding Conventions](https://docs.microsoft.com/en-us/dotnet/csharp/programming-guide/inside-a-program/coding-conventions). +Additionally, the [Unity Coding package](https://docs.unity3d.com/Packages/com.unity.coding@0.1/manual/index.html) +can be used to format, encode, and lint your code according to the standard Unity +development conventions. Be aware that these Unity conventions will supersede the Microsoft C# Coding Conventions where applicable. Please note that even if the code you are changing does not adhere to these guidelines, @@ -60,5 +60,5 @@ email us at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). ## Contribution review -Once you have a change ready following the above ground rules, simply make a -pull request in GitHub. \ No newline at end of file +Once you have a change ready following the above ground rules, simply make a +pull request in GitHub. diff --git a/README.md b/README.md index 177af0b6..a2b56ad2 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,8 @@ # Unity Robotics Hub +[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) + This is a central repository for tools, tutorials, resources, and documentation for robotic simulation in Unity. > The contents of this repository are in active development. Its features and API are subject to significant change as development progresses. @@ -60,10 +62,18 @@ Here’s a peek into what our Physics Team is hard at work on… ## FAQs [FAQs](faq.md) +## Community and Feedback + +The Unity Robotics projects are open-source and we encourage and welcome contributions. +If you wish to contribute, be sure to review our [contribution guidelines](CONTRIBUTING.md) +and [code of conduct](CODE_OF_CONDUCT.md). + ## Support -For general questions, feedback, or feature requests, connect directly with the Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). +For general questions, feedback, or feature requests, connect directly with the +Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). -For bugs or other issues, please file a GitHub issue and the Robotics team will investigate the issue as soon as possible. +For bugs or other issues, please file a GitHub issue and the Robotics team will +investigate the issue as soon as possible. ## License [Apache License 2.0](LICENSE) From 36d598a4d605d8068b448997f9632972f8da4534 Mon Sep 17 00:00:00 2001 From: Amanda <31416491+at669@users.noreply.github.com> Date: Wed, 7 Apr 2021 09:31:18 -0600 Subject: [PATCH 19/35] Adding PR template (#191) --- .github/PULL_REQUEST_TEMPLATE.md | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 9c740c07..c8773caf 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -16,7 +16,7 @@ Provide any relevant links here. ## Testing and Verification -Please describe the tests that you ran to verify your changes. Please also provide instructions, ROS packages, and Unity project files as appropriate so we can reproduce the test environment. +Please describe the tests that you ran to verify your changes. Please also provide instructions, ROS packages, and Unity project files as appropriate so we can reproduce the test environment. ### Test Configuration: - Unity Version: [e.g. Unity 2020.2.0f1] @@ -27,9 +27,8 @@ Please describe the tests that you ran to verify your changes. Please also provi ## Checklist - [ ] Ensured this PR is up-to-date with the `dev` branch - [ ] Created this PR to target the `dev` branch -- [ ] Followed the style guidelines as described in the [Contribution Guidelines](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/main/CONTRIBUTING.md) +- [ ] Followed the style guidelines as described in the [Contribution Guidelines](../CONTRIBUTING.md) - [ ] Added tests that prove my fix is effective or that my feature works -- [ ] Updated the [Changelog](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/dev/CHANGELOG.md) and described changes in the [Unreleased section](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/dev/CHANGELOG.md#unreleased) - [ ] Updated the documentation as appropriate -## Other comments \ No newline at end of file +## Other comments From 42bf426e67da2ad6bef09e5ed6c39333d6f58c4a Mon Sep 17 00:00:00 2001 From: Hamid Younesy Date: Wed, 14 Apr 2021 18:35:30 -0700 Subject: [PATCH 20/35] Unity service example (#199) * added example of implementing a UnityService * adding tutorial for unity service * added a couple comments to the example code * typos and code standard fixes * consistency: changing the service name for position_service to pos_srv * updating the tutorial to match the corresponding script --- .../robotics_demo/scripts/server_endpoint.py | 21 ++-- tutorials/ros_unity_integration/README.md | 9 +- .../ros_unity_integration/server_endpoint.md | 103 ++++++++++-------- .../ros_unity_integration/unity_service.md | 24 ++-- 4 files changed, 85 insertions(+), 72 deletions(-) diff --git a/tutorials/ros_packages/robotics_demo/scripts/server_endpoint.py b/tutorials/ros_packages/robotics_demo/scripts/server_endpoint.py index 0485d971..ababf68c 100644 --- a/tutorials/ros_packages/robotics_demo/scripts/server_endpoint.py +++ b/tutorials/ros_packages/robotics_demo/scripts/server_endpoint.py @@ -2,25 +2,24 @@ import rospy -from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService - +from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService, UnityService from robotics_demo.msg import PosRot, UnityColor -from robotics_demo.srv import PositionService +from robotics_demo.srv import PositionService, ObjectPoseService def main(): ros_node_name = rospy.get_param("/TCP_NODE_NAME", 'TCPServer') buffer_size = rospy.get_param("/TCP_BUFFER_SIZE", 1024) connections = rospy.get_param("/TCP_CONNECTIONS", 10) tcp_server = TcpServer(ros_node_name, buffer_size, connections) - - tcp_server.source_destination_dict = { - 'pos_srv': RosService('position_service', PositionService), - 'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10), - 'color': RosSubscriber('color', UnityColor, tcp_server) - } - rospy.init_node(ros_node_name, anonymous=True) - tcp_server.start() + + tcp_server.start({ + 'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10), + 'color': RosSubscriber('color', UnityColor, tcp_server), + 'pos_srv': RosService('pos_srv', PositionService), + 'obj_pose_srv': UnityService('obj_pose_srv', ObjectPoseService, tcp_server), + }) + rospy.spin() diff --git a/tutorials/ros_unity_integration/README.md b/tutorials/ros_unity_integration/README.md index 8ed68633..2f985cef 100644 --- a/tutorials/ros_unity_integration/README.md +++ b/tutorials/ros_unity_integration/README.md @@ -17,6 +17,7 @@ The `ROSConnection` plugin (also from [ROS TCP Connector](https://github.com/Uni - [ROS–Unity Integration: Publisher](publisher.md) - Adding a Publisher to a Unity Scene - [ROS–Unity Integration: Subscriber](subscriber.md) - Adding a Subscriber to a Unity Scene - [ROS–Unity Integration: Service](service.md) - Adding a Service call to a Unity Scene +- [ROS–Unity Integration: UnityService](unity_service.md) - Adding a Service that runs in a Unity Scene - [ROS–Unity Integration: Server Endpoint](server_endpoint.md) - How to write a Server Endpoint ## Example Unity Scripts @@ -26,8 +27,12 @@ Example scripts implemented in tutorials: - `unity_scripts/RosPublisherExample.cs` - Publishes the position of a GameObject every 0.5 seconds. +- `unity_scripts/RosSubscriberExample.cs` + - Subscribes to a topic that accepts color messages and uses them to change the color of a GameObject in the Unity scene. + - `unity_scripts/RosServiceExample.cs` - Returns a destination position for a GameObject to move towards each time the service is called. -- `unity_scripts/RosSubscriberExample.cs` - - Subscribes to a topic that accepts color messages and uses them to change the color of a GameObject in the Unity scene. +- `unity_scripts/RosUnityServiceExample.cs` + - Runs a service in the Unity scene that takes a GameObject's name and responds with the Pose of that object. + diff --git a/tutorials/ros_unity_integration/server_endpoint.md b/tutorials/ros_unity_integration/server_endpoint.md index d75f1ded..aa65fdc6 100644 --- a/tutorials/ros_unity_integration/server_endpoint.md +++ b/tutorials/ros_unity_integration/server_endpoint.md @@ -5,7 +5,7 @@ A walkthrough of the important components of a ROS TCP endpoint script using the The following is an example of a server endpoint Python script that: - Gets parameters from `rosparam` -- Creates corresponding ROS Publisher, Subscriber, and Service objects to interact with topics and services running in in ROS network +- Creates corresponding ROS Publisher, Subscriber, and Service objects to interact with topics and services running in ROS network - Starts TCP Server process to handle incoming and outgoing connections @@ -14,25 +14,24 @@ The following is an example of a server endpoint Python script that: import rospy -from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService - +from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService, UnityService from robotics_demo.msg import PosRot, UnityColor -from robotics_demo.srv import PositionService +from robotics_demo.srv import PositionService, ObjectPoseService def main(): ros_node_name = rospy.get_param("/TCP_NODE_NAME", 'TCPServer') buffer_size = rospy.get_param("/TCP_BUFFER_SIZE", 1024) connections = rospy.get_param("/TCP_CONNECTIONS", 10) tcp_server = TcpServer(ros_node_name, buffer_size, connections) - - tcp_server.source_destination_dict = { - 'pos_srv': RosService('position_service', PositionService), - 'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10), - 'color': RosSubscriber('color', UnityColor, tcp_server) - } - rospy.init_node(ros_node_name, anonymous=True) - tcp_server.start() + + tcp_server.start({ + 'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10), + 'color': RosSubscriber('color', UnityColor, tcp_server), + 'pos_srv': RosService('pos_srv', PositionService), + 'obj_pose_srv': UnityService('obj_pose_srv', ObjectPoseService, tcp_server), + }) + rospy.spin() @@ -43,12 +42,22 @@ if __name__ == "__main__": ## Import Statements for Services and Messages ```python -from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService - +from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService, UnityService from robotics_demo.msg import PosRot, UnityColor -from robotics_demo.srv import PositionService +from robotics_demo.srv import PositionService, ObjectPoseService +``` + +## Creating the Server + +Requires: + +- The ROS node name + +```python + tcp_server = TcpServer(ros_node_name, buffer_size, connections) ``` +The `ros_node_name` argument is required and the `buffer_size` and `connections` are optional. They are set to `1024` and `10` by default if not provided in the constructor arguments. ## Instantiate the ROS Node @@ -56,16 +65,34 @@ from robotics_demo.srv import PositionService rospy.init_node(ros_node_name, anonymous=True) ``` +## Starting the Server + +```python + tcp_server.start({ + 'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10), + 'color': RosSubscriber('color', UnityColor, tcp_server), + 'pos_srv': RosService('pos_srv', PositionService), + 'obj_pose_srv': UnityService('obj_pose_srv', ObjectPoseService, tcp_server), + }) + + rospy.spin() +``` + +## Source Destination Dictionary + +The argument to start() is a dictionary keyed by topic or service with the corresponding ROS communication class as the value. The dictionary is used by the TCP server to direct messages to and from the ROS network. + ## ROS Publisher -A ROS Publisher requires three components: +A ROS Publisher allows a Unity component to send messages on a given topic to other ROS nodes. It requires three components: - Topic name - ROS message class generated from running `catkin_make` command -- Queue size +- Queue size (optional) `RosPublisher('pos_rot', PosRot, queue_size=10)` + ## ROS Subscriber -A ROS Subscriber requires three components: +A ROS Subscriber allows a Unity component to receive messages from other ROS nodes on a given topic. It requires three components: - Topic name - ROS message class generated from running `catkin_make` command @@ -74,43 +101,25 @@ A ROS Subscriber requires three components: `RosSubscriber('color', UnityColor, tcp_server)` ## ROS Service -A ROS Service requires two components: +A ROS Service is similar to a RosPublisher, in that a Unity component sends a Request message to another ROS node. Unlike a Publisher, the Unity component then waits for a Response back. It requires two components: - Service name - ROS Service class generated from running `catkin_make` command -`RosService('position_service', PositionService)` - -## Creating the Server - -Requires: - -- The ROS node name - -```python - tcp_server = TcpServer(ros_node_name) -``` +`RosService('pos_srv', PositionService)` -## Source Destination Dictionary +## Unity Service -Create a dictionary keyed by topic or service with the corresponding ROS communication class as the value. The dictionary is used by the TCP server to direct messages to and from the ROS network. +A Unity Service is similar to a RosSubscriber, in that a Unity component receives a Request message from another ROS node. It then sends a Response back. It requires three components: -```python - tcp_server.source_destination_dict = { - 'pos_srv': RosService('position_service', PositionService), - 'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10), - 'color': RosSubscriber('color', Color, tcp_server), - } -``` +- Service name +- ROS Service class generated from running `catkin_make` command +- The tcp server that will connect to Unity -## Starting the Server +`UnityService('obj_pose_srv', ObjectPoseService, tcp_server)` -```python - tcp_server.start() - rospy.spin() - -``` +## Parameters The following parameters can be hardcoded, but for the sake of portability, we recommend setting the parameters using the `rosparam set` command, or a `rosparam` YAML file. @@ -130,7 +139,7 @@ An example launch file that will set the appropriate ROSPARAM values required fo ``` - + @@ -142,4 +151,4 @@ An example launch file that will set the appropriate ROSPARAM values required fo -``` +``` \ No newline at end of file diff --git a/tutorials/ros_unity_integration/unity_service.md b/tutorials/ros_unity_integration/unity_service.md index 15235896..366656e5 100644 --- a/tutorials/ros_unity_integration/unity_service.md +++ b/tutorials/ros_unity_integration/unity_service.md @@ -11,7 +11,7 @@ Create a simple Unity scene which create a [Service](http://wiki.ros.org/Service - Follow the [ROS–Unity Initial Setup](setup.md) guide. - Open a new terminal window, navigate to your ROS workspace, and run the following commands: - + ```bash source devel/setup.bash rosrun robotics_demo server_endpoint.py @@ -26,7 +26,7 @@ Once the server_endpoint has started, it will print something similar to `[INFO] ![](images/generate_messages_2.png) - The generated files will be saved in the default directory `Assets/RosMessages/RoboticsDemo/srv`. - + - Create a new C# script and name it `RosUnityServiceExample.cs` - Paste the following code into `RosUnityServiceExample.cs` - **Note:** This script can be found at `tutorials/ros_unity_integration/unity_scripts`. @@ -65,13 +65,13 @@ public class RosUnityServiceExample : MonoBehaviour MObjectPoseServiceResponse objectPoseResponse = new MObjectPoseServiceResponse(); // Find a game object with the requested name GameObject gameObject = GameObject.Find(request.object_name); - if (gameObject) + if (gameObject) { // Fill-in the response with the object pose converted from Unity coordinate to ROS coordinate system objectPoseResponse.object_pose.position = gameObject.transform.position.To(); objectPoseResponse.object_pose.orientation = gameObject.transform.rotation.To(); } - + return objectPoseResponse; } } @@ -79,8 +79,8 @@ public class RosUnityServiceExample : MonoBehaviour - From the main menu bar, open `Robotics/ROS Settings`, and change the `ROS IP Address` variable to the ROS IP. - Create an empty GameObject and name it `UnityService`. -- Attach the `RosUnityServiceExample` script to the `UnityService` GameObject. -- Pressing play in the Editor should start running as a ROS node, waiting to accept ObjectPose requests. Once a connection to ROS has been established, a message will be printed on the ROS terminal similar to `Connection from 172.17.0.1`. +- Attach the `RosUnityServiceExample` script to the `UnityService` GameObject. +- Pressing play in the Editor should start running as a ROS node, waiting to accept ObjectPose requests. Once a connection to ROS has been established, a message will be printed on the ROS terminal similar to `ROS-Unity Handshake received, will connect to 192.168.50.130:5005`. ## Start the Client @@ -94,12 +94,12 @@ public class RosUnityServiceExample : MonoBehaviour ```bash Requesting pose for Cube - Pose for Cube: - position: + Pose for Cube: + position: x: 0.0 y: -1.0 z: 0.20000000298023224 - orientation: + orientation: x: 0.0 y: -0.0 z: 0.0 @@ -113,12 +113,12 @@ You may replace `Cube` with the name of any other GameObject currently present i rosservice call /obj_pose_srv Cube ``` ```bash - object_pose: - position: + object_pose: + position: x: 0.0 y: -1.0 z: 0.20000000298023224 - orientation: + orientation: x: 0.0 y: -0.0 z: 0.0 From dd47436688b883e3bface99c11a97d7904f6d72b Mon Sep 17 00:00:00 2001 From: peifeng-unity <56408141+peifeng-unity@users.noreply.github.com> Date: Fri, 16 Apr 2021 10:27:54 -0700 Subject: [PATCH 21/35] Update the list of new physics features (#204) * add new sections for the released new physics features Co-authored-by: Amanda <31416491+at669@users.noreply.github.com> --- README.md | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index a2b56ad2..4b76a093 100644 --- a/README.md +++ b/README.md @@ -50,12 +50,15 @@ Use articulation bodies to easily prototype industrial designs with realistic mo - [Unity Industrial Simulation](https://unity.com/products/unity-simulation) - [Unity Computer Vision](https://unity.com/computer-vision) -## Coming Soon - New Physics Features in Unity! +## New Physics Features in Unity +### New Features +- **Contact Modification API** This API will allow users to define custom contact reactions, such as ignoring subsets of contact points, in order to help simulate holes, slippery surfaces, soft contacts, and more. It is available in Unity versions **2021.2a12+**. [Read more about the new Contact Modification API](https://forum.unity.com/threads/experimental-contacts-modification-api.924809/). +- **Collision detection modes exposed for ArticulationBody: discrete, sweep-based CCD, and speculative CCD**. New continuous collision detection (CCD) modes will ensure that fast-moving objects collide with objects, instead of tunneling or passing through those objects, which can happen in the default “discrete” mode. This API is available in Unity versions **2020.3.5f1+**. [Read more about continuous collision detection](https://docs.unity3d.com/2020.3/Documentation/ScriptReference/ArticulationBody-collisionDetectionMode.html). + +### Coming Soon Here’s a peek into what our Physics Team is hard at work on… -- **Contact Modification API**. This API will allow users to define custom contact reactions, such as ignoring subsets of contact points, in order to help simulate holes, slippery surfaces, soft contacts, and more. [Read more about the new Contact Modification API](https://forum.unity.com/threads/experimental-contacts-modification-api.924809/). - **Wheel Collider shapes**. This feature will allow the user to specify the shape of the collider to be used for collision detection. Currently the collider shape is fixed to a cylinder, and collision detection is performed by casting a ray from the center of the cylinder. Custom shapes will improve the accuracy of simulating wheels over rough terrains, holes, etc. [Read more about Wheel Collider](https://docs.unity3d.com/Manual/class-WheelCollider.html). -- **Collision detection modes exposed for ArticulationBody: discrete, sweep-based ccd, and speculative ccd**. New continuous collision detection (ccd) modes will ensure that fast-moving objects collide with objects, instead of tunneling or passing through those objects, which can happen in the default “discrete” mode. [Read more about continuous collision detection](https://docs.unity3d.com/Manual/ContinuousCollisionDetection.html). - **Force/Torque Sensor API**. This API will allow users to get the force and torque acting on an articulation body (useful for simulating a force/torque sensor!), as well as to get the motor torque applied by an articulation drive. - **Query primitives**. These simple, GameObject-less shapes allow for collision detection without requiring simulation (i.e., without calling Physics.Simulate). This feature will allow users to initialize objects in feasible locations, and can also be used for motion planning. From efb86a965070cfa7106e41a70d0da7586e03cc71 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Wed, 21 Apr 2021 11:42:36 -0700 Subject: [PATCH 22/35] Update to 0.3.0 (#207) --- .../pick_and_place/PickAndPlaceProject/Packages/manifest.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json index 812be142..c0a03b65 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json +++ b/tutorials/pick_and_place/PickAndPlaceProject/Packages/manifest.json @@ -5,8 +5,8 @@ "com.unity.ide.visualstudio": "2.0.3", "com.unity.ide.vscode": "1.2.2", "com.unity.robotics.ros-tcp-connector": - "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#dev", - "com.unity.robotics.urdf-importer": "https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.2.0", + "https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#v0.3.0", + "com.unity.robotics.urdf-importer": "https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.3.0", "com.unity.test-framework": "1.1.18", "com.unity.textmeshpro": "3.0.1", "com.unity.timeline": "1.4.3", From 15c930531a004669fdbaa602c9b82a9686a44a49 Mon Sep 17 00:00:00 2001 From: "Devin Miller (Unity)" Date: Mon, 26 Apr 2021 11:46:23 -0700 Subject: [PATCH 23/35] Removing old version tags from git URLs in quick_setup Addressing user issue #209. Our quick_setup guide was still recommending users check out v.0.2.0 of our packages. Removed the reference to a specific package version and instead included a note that users can explicitly declare a package version if desired. Tested by making the change locally in the package manifest. --- tutorials/pick_and_place/2_ros_tcp.md | 2 +- tutorials/quick_setup.md | 6 +++--- tutorials/ros_unity_integration/README.md | 2 ++ tutorials/ros_unity_integration/network.md | 2 +- tutorials/ros_unity_integration/publisher.md | 2 ++ tutorials/ros_unity_integration/subscriber.md | 2 ++ 6 files changed, 11 insertions(+), 5 deletions(-) diff --git a/tutorials/pick_and_place/2_ros_tcp.md b/tutorials/pick_and_place/2_ros_tcp.md index dbec6059..57a30a93 100644 --- a/tutorials/pick_and_place/2_ros_tcp.md +++ b/tutorials/pick_and_place/2_ros_tcp.md @@ -140,7 +140,7 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n - If you are **not** running ROS services in a Docker container, replace the `ROS IP Address` value with the IP address of your ROS machine. Ensure that the `Host Port` is set to `10000`. - - If you **are** running ROS services in a Docker container, fill `ROS IP Address` and `Override Unity IP Address` with the loopback IP address `127.0.0.1`. Otherwise, leave the `Override Unity IP Address` field empty. + - If you **are** running ROS services in a Docker container, fill `ROS IP Address` with the loopback IP address `127.0.0.1` and the `Override Unity IP Address` as your local machine's IP address. Otherwise, leave the `Override Unity IP Address` field empty. ![](img/2_settings.png) diff --git a/tutorials/quick_setup.md b/tutorials/quick_setup.md index c240031c..2945b708 100644 --- a/tutorials/quick_setup.md +++ b/tutorials/quick_setup.md @@ -11,9 +11,9 @@ This page provides brief instructions on installing the Unity Robotics packages. ![](../images/packman.png) -1. Enter the git URL for the desired package. - 1. For the [ROS-TCP-Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector), enter `https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector#v0.2.0`. - 1. For the [URDF-Importer](https://github.com/Unity-Technologies/URDF-Importer), enter `https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer#v0.2.0`. +1. Enter the git URL for the desired package. Note: you can append a version tag to the end of the git url, like `#v0.2.0` or `#v0.3.0`, to declare a specific package version, or exclude the tag to get the latest from the package's `main` branch. + 1. For the [ROS-TCP-Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector), enter `https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector`. + 1. For the [URDF-Importer](https://github.com/Unity-Technologies/URDF-Importer), enter `https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer`. 1. Click `Add`. diff --git a/tutorials/ros_unity_integration/README.md b/tutorials/ros_unity_integration/README.md index 2f985cef..45997364 100644 --- a/tutorials/ros_unity_integration/README.md +++ b/tutorials/ros_unity_integration/README.md @@ -14,6 +14,8 @@ The `ROSConnection` plugin (also from [ROS TCP Connector](https://github.com/Uni ## Tutorials +- [ROS–Unity Integration: Initial Setup](setup.md) - ROS-Unity Initial Setup +- [ROS–Unity Integration: Network Description](network.md) - Description of network settings and troubleshooting - [ROS–Unity Integration: Publisher](publisher.md) - Adding a Publisher to a Unity Scene - [ROS–Unity Integration: Subscriber](subscriber.md) - Adding a Subscriber to a Unity Scene - [ROS–Unity Integration: Service](service.md) - Adding a Service call to a Unity Scene diff --git a/tutorials/ros_unity_integration/network.md b/tutorials/ros_unity_integration/network.md index cefb66ed..5625199c 100644 --- a/tutorials/ros_unity_integration/network.md +++ b/tutorials/ros_unity_integration/network.md @@ -30,7 +30,7 @@ The minimum settings required for Unity to communicate with ROS is to set the `R ## If Using Docker -The container will need to be started with the following arguments to forward the ports used for communication between ROS and Unity. +The container will need to be started with the following arguments to forward the ports used for communication between ROS and Unity. `-p 10000:10000 -p 5005:5005` diff --git a/tutorials/ros_unity_integration/publisher.md b/tutorials/ros_unity_integration/publisher.md index f736af41..14ef0e7b 100644 --- a/tutorials/ros_unity_integration/publisher.md +++ b/tutorials/ros_unity_integration/publisher.md @@ -99,6 +99,8 @@ public class RosPublisherExample : MonoBehaviour - Pressing play in the Editor should publish a message to the terminal running the `rostopic echo pos_rot` command every 0.5 seconds +> Please reference [networking troubleshooting](network.md) doc if any errors are thrown. + ![](images/tcp_1.gif) Continue to the [ROS Subscriber](subscriber.md) tutorial. \ No newline at end of file diff --git a/tutorials/ros_unity_integration/subscriber.md b/tutorials/ros_unity_integration/subscriber.md index ac5164e9..e9c79fe6 100644 --- a/tutorials/ros_unity_integration/subscriber.md +++ b/tutorials/ros_unity_integration/subscriber.md @@ -66,6 +66,8 @@ public class RosSubscriberExample : MonoBehaviour ### In ROS Terminal Window - After the scene has entered Play mode, run the following command: `rosrun robotics_demo color_publisher.py` to change the color of the cube GameObject in Unity to a random color +> Please reference [networking troubleshooting](network.md) doc if any errors are thrown. + ![](images/tcp_2.gif) Continue to the [ROS–Unity Integration Service](service.md). \ No newline at end of file From 5f6fd0da76197b1f354e12d57537eb109b04db9d Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Fri, 7 May 2021 11:57:31 -0700 Subject: [PATCH 24/35] Updating tutorials to the glorious new world (#211) --- tutorials/ros_unity_integration/subscriber.md | 5 ----- tutorials/ros_unity_integration/unity_service.md | 2 +- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/tutorials/ros_unity_integration/subscriber.md b/tutorials/ros_unity_integration/subscriber.md index e9c79fe6..6f0cd488 100644 --- a/tutorials/ros_unity_integration/subscriber.md +++ b/tutorials/ros_unity_integration/subscriber.md @@ -56,11 +56,6 @@ public class RosSubscriberExample : MonoBehaviour - Attach the `RosSubscriberExample` script to the `RosSubscriber` GameObject and drag the cube GameObject onto the `cube` parameter in the Inspector window. - From the Unity menu bar, open `Robotics/ROS Settings`, and set the `ROS IP Address` variable to your ROS IP. - - Unlike the previous tutorial, using a subscriber requires an incoming connection from ROS to Unity. You may need to adjust your firewall settings for this to work. - - The IP for Unity to listen on should be determined automatically, but if you're having trouble, you can set it manually in the `Override Unity IP` field. Finding the IP address of your local machine (the one running Unity) depends on your operating system. - - On a Mac, open `System Preferences > Network`. Your IP address should be listed on the active connection. - - On Windows, click the Wi-Fi icon on the taskbar, and open `Properties`. Your IP address should be listed near the bottom, next to "IPv4 address." - - Press play in the editor ### In ROS Terminal Window diff --git a/tutorials/ros_unity_integration/unity_service.md b/tutorials/ros_unity_integration/unity_service.md index 366656e5..eb61af4f 100644 --- a/tutorials/ros_unity_integration/unity_service.md +++ b/tutorials/ros_unity_integration/unity_service.md @@ -80,7 +80,7 @@ public class RosUnityServiceExample : MonoBehaviour - From the main menu bar, open `Robotics/ROS Settings`, and change the `ROS IP Address` variable to the ROS IP. - Create an empty GameObject and name it `UnityService`. - Attach the `RosUnityServiceExample` script to the `UnityService` GameObject. -- Pressing play in the Editor should start running as a ROS node, waiting to accept ObjectPose requests. Once a connection to ROS has been established, a message will be printed on the ROS terminal similar to `ROS-Unity Handshake received, will connect to 192.168.50.130:5005`. +- Pressing play in the Editor should start running as a ROS node, waiting to accept ObjectPose requests. Once a connection to ROS has been established, a message will be printed on the ROS terminal similar to `Connection from 172.17.0.1`. ## Start the Client From 279740b5360d538b0c6715a832b076db01c78802 Mon Sep 17 00:00:00 2001 From: peifeng-unity <56408141+peifeng-unity@users.noreply.github.com> Date: Wed, 12 May 2021 10:37:57 -0700 Subject: [PATCH 25/35] add changelog (#218) add changelog --- .github/PULL_REQUEST_TEMPLATE.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index c8773caf..4d51e1fc 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -27,8 +27,9 @@ Please describe the tests that you ran to verify your changes. Please also provi ## Checklist - [ ] Ensured this PR is up-to-date with the `dev` branch - [ ] Created this PR to target the `dev` branch -- [ ] Followed the style guidelines as described in the [Contribution Guidelines](../CONTRIBUTING.md) +- [ ] Followed the style guidelines as described in the [Contribution Guidelines](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/main/CONTRIBUTING.md) - [ ] Added tests that prove my fix is effective or that my feature works +- [ ] Updated the [Changelog](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/dev/CHANGELOG.md) and described changes in the [Unreleased section](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/dev/CHANGELOG.md#unreleased) - [ ] Updated the documentation as appropriate ## Other comments From 3701595f797ed23192c013250c07e73791b35617 Mon Sep 17 00:00:00 2001 From: Amanda <31416491+at669@users.noreply.github.com> Date: Thu, 13 May 2021 09:15:07 -0600 Subject: [PATCH 26/35] Forum links to README, config.yml (#219) --- README.md | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 4b76a093..660aff8f 100644 --- a/README.md +++ b/README.md @@ -72,11 +72,13 @@ If you wish to contribute, be sure to review our [contribution guidelines](CONTR and [code of conduct](CODE_OF_CONDUCT.md). ## Support -For general questions, feedback, or feature requests, connect directly with the -Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). -For bugs or other issues, please file a GitHub issue and the Robotics team will -investigate the issue as soon as possible. +For questions or discussions about Unity Robotics package installations or how to best set up and integrate your robotics projects, please create a new thread on the [Unity Robotics forum](https://forum.unity.com/forums/robotics.623/) and make sure to include as much detail as possible. + +For feature requests, bugs, or other issues, please file a [GitHub issue](https://github.com/Unity-Technologies/Unity-Robotics-Hub/issues) using the provided templates and the Robotics team will investigate as soon as possible. + +For any other questions or feedback, connect directly with the +Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). ## License [Apache License 2.0](LICENSE) From 0d04b67020301eb52b4335b6fe149a5f831525f4 Mon Sep 17 00:00:00 2001 From: Shuo Diao Date: Wed, 26 May 2021 09:54:44 -0700 Subject: [PATCH 27/35] Add linter (#227) * Add linter and pre-commit hooks * Formatting * Remove setting up ruby --- .github/PULL_REQUEST_TEMPLATE.md | 2 +- .gitignore | 2 +- CONTRIBUTING.md | 16 ++--- README.md | 10 ++-- tutorials/pick_and_place/2_ros_tcp.md | 58 +++++++++---------- tutorials/pick_and_place/4_pick_and_place.md | 34 +++++------ .../PickAndPlaceMessageGenerationtests.cs | 4 +- tutorials/pick_and_place/README.md | 8 +-- .../Scripts/SourceDestinationPublisher.cs | 16 ++--- .../Scripts/TrajectoryPlanner.cs | 30 +++++----- .../Scripts_Part4/RealSimPickAndPlace.cs | 14 ++--- tutorials/quick_setup.md | 6 +- tutorials/ros_unity_integration/network.md | 2 +- tutorials/ros_unity_integration/publisher.md | 8 +-- .../ros_unity_integration/server_endpoint.md | 8 +-- tutorials/ros_unity_integration/subscriber.md | 4 +- .../unity_scripts/RosServiceExample.cs | 4 +- .../unity_scripts/RosSubscriberExample.cs | 36 ++++++------ .../ros_unity_integration/unity_service.md | 22 +++---- tutorials/urdf_importer/urdf_appendix.md | 42 +++++++------- tutorials/urdf_importer/urdf_tutorial.md | 10 ++-- 21 files changed, 168 insertions(+), 168 deletions(-) diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 4d51e1fc..c3ad7112 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -16,7 +16,7 @@ Provide any relevant links here. ## Testing and Verification -Please describe the tests that you ran to verify your changes. Please also provide instructions, ROS packages, and Unity project files as appropriate so we can reproduce the test environment. +Please describe the tests that you ran to verify your changes. Please also provide instructions, ROS packages, and Unity project files as appropriate so we can reproduce the test environment. ### Test Configuration: - Unity Version: [e.g. Unity 2020.2.0f1] diff --git a/.gitignore b/.gitignore index ba15c0cb..0653b96a 100644 --- a/.gitignore +++ b/.gitignore @@ -2,4 +2,4 @@ .swp .idea .vscode/ -tutorials/pick_and_place/PickAndPlaceProject/Packages/packages-lock.json +tutorials/pick_and_place/PickAndPlaceProject/Packages/packages-lock.json diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 7cc07605..18edb9f8 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -1,7 +1,7 @@ # Contribution Guidelines -Thank you for your interest in contributing to Unity Robotics! To facilitate your -contributions, we've outlined a brief set of guidelines to ensure that your extensions +Thank you for your interest in contributing to Unity Robotics! To facilitate your +contributions, we've outlined a brief set of guidelines to ensure that your extensions can be easily integrated. ## Communication @@ -40,10 +40,10 @@ We run continuous integration on all PRs; all tests must be passing before the P All Python code should follow the [PEP 8 style guidelines](https://pep8.org/). -All C# code should follow the [Microsoft C# Coding Conventions](https://docs.microsoft.com/en-us/dotnet/csharp/programming-guide/inside-a-program/coding-conventions). -Additionally, the [Unity Coding package](https://docs.unity3d.com/Packages/com.unity.coding@0.1/manual/index.html) -can be used to format, encode, and lint your code according to the standard Unity -development conventions. Be aware that these Unity conventions will supersede the +All C# code should follow the [Microsoft C# Coding Conventions](https://docs.microsoft.com/en-us/dotnet/csharp/programming-guide/inside-a-program/coding-conventions). +Additionally, the [Unity Coding package](https://docs.unity3d.com/Packages/com.unity.coding@0.1/manual/index.html) +can be used to format, encode, and lint your code according to the standard Unity +development conventions. Be aware that these Unity conventions will supersede the Microsoft C# Coding Conventions where applicable. Please note that even if the code you are changing does not adhere to these guidelines, @@ -60,5 +60,5 @@ email us at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). ## Contribution review -Once you have a change ready following the above ground rules, simply make a -pull request in GitHub. +Once you have a change ready following the above ground rules, simply make a +pull request in GitHub. \ No newline at end of file diff --git a/README.md b/README.md index 660aff8f..fe327a8c 100644 --- a/README.md +++ b/README.md @@ -19,7 +19,7 @@ Simulation plays an important role in robotics development, and we’re here to | [Quick Installation Instructions](tutorials/quick_setup.md) | Brief steps on installing the Unity Robotics packages | | [Pick-and-Place Demo](tutorials/pick_and_place/README.md) | A complete end-to-end demonstration, including how to set up the Unity environment, how to import a robot from URDF, and how to set up two-way communication with ROS for control | | [ROS–Unity Integration](tutorials/ros_unity_integration/README.md) | A set of component-level tutorials showing how to set up communication between ROS and Unity | -| [URDF Importer](tutorials/urdf_importer/urdf_tutorial.md) | Steps on using the Unity package for loading [URDF](http://wiki.ros.org/urdf) files | +| [URDF Importer](tutorials/urdf_importer/urdf_tutorial.md) | Steps on using the Unity package for loading [URDF](http://wiki.ros.org/urdf) files | | [Articulations Robot Demo](https://github.com/Unity-Technologies/articulations-robot-demo) | A robot simulation demonstrating Unity's new physics solver (no ROS dependency) @@ -43,7 +43,7 @@ Unite Now 2020: Simulating Robots with ROS and Unity [video](https://resources.u - (August 26, 2020) Announcing Unity Robotic Simulation [blog post](https://unity.com/solutions/automotive-transportation-manufacturing/robotics) - (May 20, 2020) -Use articulation bodies to easily prototype industrial designs with realistic motion and behavior [blog post](https://blogs.unity3d.com/2020/05/20/use-articulation-bodies-to-easily-prototype-industrial-designs-with-realistic-motion-and-behavior/) +Use articulation bodies to easily prototype industrial designs with realistic motion and behavior [blog post](https://blogs.unity3d.com/2020/05/20/use-articulation-bodies-to-easily-prototype-industrial-designs-with-realistic-motion-and-behavior/) ### More from Unity @@ -67,8 +67,8 @@ Here’s a peek into what our Physics Team is hard at work on… ## Community and Feedback -The Unity Robotics projects are open-source and we encourage and welcome contributions. -If you wish to contribute, be sure to review our [contribution guidelines](CONTRIBUTING.md) +The Unity Robotics projects are open-source and we encourage and welcome contributions. +If you wish to contribute, be sure to review our [contribution guidelines](CONTRIBUTING.md) and [code of conduct](CODE_OF_CONDUCT.md). ## Support @@ -77,7 +77,7 @@ For questions or discussions about Unity Robotics package installations or how t For feature requests, bugs, or other issues, please file a [GitHub issue](https://github.com/Unity-Technologies/Unity-Robotics-Hub/issues) using the provided templates and the Robotics team will investigate as soon as possible. -For any other questions or feedback, connect directly with the +For any other questions or feedback, connect directly with the Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). ## License diff --git a/tutorials/pick_and_place/2_ros_tcp.md b/tutorials/pick_and_place/2_ros_tcp.md index 57a30a93..b92620e0 100644 --- a/tutorials/pick_and_place/2_ros_tcp.md +++ b/tutorials/pick_and_place/2_ros_tcp.md @@ -25,7 +25,7 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n ## The Unity Side -1. If you have not already completed the steps in [Part 0](0_ros_setup.md) to set up your ROS workspace and [Part 1](1_urdf.md) to set up the Unity project, do so now. +1. If you have not already completed the steps in [Part 0](0_ros_setup.md) to set up your ROS workspace and [Part 1](1_urdf.md) to set up the Unity project, do so now. 1. If the PickAndPlaceProject Unity project is not already open, select and open it from the Unity Hub. @@ -45,36 +45,36 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n ![](img/2_menu.png) - In the ROS Message Browser window, click `Browse` next to the ROS message path. Navigate to and select the ROS directory of this cloned repository (`Unity-Robotics-Hub/tutorials/pick_and_place/ROS/`). This window will populate with all msg and srv files found in this directory. - + In the ROS Message Browser window, click `Browse` next to the ROS message path. Navigate to and select the ROS directory of this cloned repository (`Unity-Robotics-Hub/tutorials/pick_and_place/ROS/`). This window will populate with all msg and srv files found in this directory. + ![](img/2_browser.png) > Note: If any of these ROS directories appear to be empty, you can run the command `git submodule update --init --recursive` to download the packages via Git submodules. - + Under `ROS/src/moveit_msgs/msg`, scroll to `RobotTrajectory.msg`, and click its `Build msg` button. The button text will change to "Rebuild msg" when it has finished building. ![](img/2_robottraj.png) - One new C# script should populate the `Assets/RosMessages/Moveit/msg` directory: MRobotTrajectory. This name is the same as the message you built, with an "M" prefix (for message). - -1. Next, the custom message scripts for this tutorial will need to be generated. + +1. Next, the custom message scripts for this tutorial will need to be generated. Still in the ROS Message Browser window, expand `ROS/src/niryo_moveit/msg` to view the msg files listed. Next to msg, click `Build 2 msgs`. ![](img/2_msg.png) - + - Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/msg` directory: MNiryoMoveitJoints and MNiryoTrajectory. MNiryoMoveitJoints describes a value for each joint in the Niryo arm as well as poses for the target object and target goal. MNiryoTrajectory describes a list of RobotTrajectory values, which will hold the calculated trajectories for the pick-and-place task. - + > MessageGeneration generates a C# class from a ROS msg file with protections for use of C# reserved keywords and conversion to C# datatypes. Learn more about [ROS Messages](https://wiki.ros.org/Messages). -1. Finally, now that the messages have been generated, we will create the service for moving the robot. +1. Finally, now that the messages have been generated, we will create the service for moving the robot. - Still in the ROS Message Browser window, expand `ROS/src/niryo_moveit/srv` to view the srv file listed. Next to srv, click `Build 1 srv`. + Still in the ROS Message Browser window, expand `ROS/src/niryo_moveit/srv` to view the srv file listed. Next to srv, click `Build 1 srv`. ![](img/2_srv.png) - - Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/srv` directory: MMoverServiceRequest and MMoverServiceResponse. These files describe the expected input and output formats for the service requests and responses when calculating trajectories. - + - Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/srv` directory: MMoverServiceRequest and MMoverServiceResponse. These files describe the expected input and output formats for the service requests and responses when calculating trajectories. + > MessageGeneration generates two C# classes, a request and response, from a ROS srv file with protections for use of C# reserved keywords and conversion to C# datatypes. Learn more about [ROS Services](https://wiki.ros.org/Services). You can now close the ROS Message Browser window. @@ -115,24 +115,24 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n } ``` - > This function first takes in the current joint target values. Then, it grabs the poses of the `target` and the `targetPlacement` objects, adds them to the newly created message `sourceDestinationMessage`, and calls `Send()` to send this information to the ROS topic `topicName` (defined as `"SourceDestination_input"`). + > This function first takes in the current joint target values. Then, it grabs the poses of the `target` and the `targetPlacement` objects, adds them to the newly created message `sourceDestinationMessage`, and calls `Send()` to send this information to the ROS topic `topicName` (defined as `"SourceDestination_input"`). > Note: Going from Unity world space to ROS world space requires a conversion. Unity's `(x,y,z)` is equivalent to the ROS `(z,-x,y)` coordinate. These conversions are provided via the [ROSGeometry component](https://github.com/Unity-Technologies/ROS-TCP-Connector/blob/main/ROSGeometry.md) in the ROS-TCP-Connector package. -1. Return to the Unity Editor. Now that the message contents have been defined and the publisher script added, it needs to be added to the Unity world to run its functionality. +1. Return to the Unity Editor. Now that the message contents have been defined and the publisher script added, it needs to be added to the Unity world to run its functionality. - Right click in the Hierarchy window and select "Create Empty" to add a new empty GameObject. Name it `Publisher`. Add the newly created SourceDestinationPublisher component to the Publisher GameObject by selecting the Publisher object. Click "Add Component" in the Inspector, and begin typing "SourceDestinationPublisher." Select the component when it appears. + Right click in the Hierarchy window and select "Create Empty" to add a new empty GameObject. Name it `Publisher`. Add the newly created SourceDestinationPublisher component to the Publisher GameObject by selecting the Publisher object. Click "Add Component" in the Inspector, and begin typing "SourceDestinationPublisher." Select the component when it appears. > Note: Alternatively, you can drag the script from the Project window onto the Publisher object in the Hierarchy window. ![](img/2_sourcedest.gif) -1. Note that this component shows empty member variables in the Inspector window, which need to be assigned. +1. Note that this component shows empty member variables in the Inspector window, which need to be assigned. Select the Target object in the Hierarchy and assign it to the `Target` field in the Publisher. Similarly, assign the TargetPlacement object to the `TargetPlacement` field. Assign the niryo_one robot to the `Niryo One` field. ![](img/2_target.gif) -1. Next, the ROS TCP connection needs to be created. Select `Robotics -> ROS Settings` from the top menu bar. +1. Next, the ROS TCP connection needs to be created. Select `Robotics -> ROS Settings` from the top menu bar. In the ROS Settings window, the `ROS IP Address` should be the IP address of your ROS machine (*not* the one running Unity). @@ -145,14 +145,14 @@ To enable communication between Unity and ROS, a TCP endpoint running as a ROS n ![](img/2_settings.png) Opening the ROS Settings has created a ROSConnectionPrefab in `Assets/Resources` with the user-input settings. When the static `ROSConnection.instance` is referenced in a script, if a `ROSConnection` instance is not already present, the prefab will be instantiated in the Unity scene, and the connection will begin. - + > Note: While using the ROS Settings menu is the suggested workflow as of this version, you may still manually create a GameObject with an attached ROSConnection component. -1. Next, we will add a UI element that will allow user input to trigger the `Publish()` function. In the Hierarchy window, right click to add a new UI > Button. Note that this will create a new Canvas parent as well. +1. Next, we will add a UI element that will allow user input to trigger the `Publish()` function. In the Hierarchy window, right click to add a new UI > Button. Note that this will create a new Canvas parent as well. > Note: In the `Game` view, you will see the button appear in the bottom left corner as an overlay. In `Scene` view the button will be rendered on a canvas object that may not be visible. - - > Note: In case the Button does not start in the bottom left, it can be moved by setting the `Pos X` and `Pos Y` values in its Rect Transform component. For example, setting its Position to `(-200, -200, 0)` would set its position to the bottom right area of the screen. - + + > Note: In case the Button does not start in the bottom left, it can be moved by setting the `Pos X` and `Pos Y` values in its Rect Transform component. For example, setting its Position to `(-200, -200, 0)` would set its position to the bottom right area of the screen. + 1. Select the newly made Button object, and scroll to see the Button component in the Inspector. Click the `+` button under the empty `OnClick()` header to add a new event. Select the `Publisher` object in the Hierarchy window and drag it into the new OnClick() event, where it says `None (Object)`. Click the dropdown where it says `No Function`. Select SourceDestinationPublisher > `Publish()`. ![](img/2_onclick.png) @@ -175,7 +175,7 @@ Most of the ROS setup has been provided via the `niryo_moveit` package. This sec roslaunch niryo_moveit part_2.launch ``` - > Note: Running `roslaunch` automatically starts [ROS Core](http://wiki.ros.org/roscore) if it is not already running. + > Note: Running `roslaunch` automatically starts [ROS Core](http://wiki.ros.org/roscore) if it is not already running. > Note: This launch file has been copied below for reference. The server_endpoint and trajectory_subscriber nodes are launched from this file, and the ROS params (set up in [Part 0](0_ros_setup.md)) are loaded from this command. The launch files for this project are available in the package's `launch` directory, i.e. `src/niryo_moveit/launch/`. @@ -187,12 +187,12 @@ Most of the ROS setup has been provided via the `niryo_moveit` package. This sec ``` - This launch will print various messages to the console, including the set parameters and the nodes launched. - + This launch will print various messages to the console, including the set parameters and the nodes launched. + Ensure that the `process[server_endpoint]` and `process[trajectory_subscriber]` were successfully started, and that a message similar to `[INFO] [1603488341.950794]: Starting server on 192.168.50.149:10000` is printed. 1. Return to Unity, and press Play. Click the UI Button in the Game view to call SourceDestinationPublisher's `Publish()` function, publishing the associated data to the ROS topic. View the terminal in which the `roslaunch` command is running. It should now print `I heard:` with the data. - + ROS and Unity have now successfully connected! ![](img/2_echo.png) @@ -203,8 +203,8 @@ ROS and Unity have now successfully connected! - If the error `[rosrun] Found the following, but they're either not files, or not executable: server_endpoint.py` appears, the Python script may need to be marked as executable via `chmod +x Unity-Robotics-Hub/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py`. - `...failed because unknown error handler name 'rosmsg'` This is due to a bug in an outdated package version. Try running `sudo apt-get update && sudo apt-get upgrade` to upgrade. - -- If Unity fails to find a network connection, ensure that the ROS IP address is entered correctly as the ROS IP Address in the RosConnect in Unity, and that the `src/niryo_moveit/config/params.yaml` values are set correctly. + +- If Unity fails to find a network connection, ensure that the ROS IP address is entered correctly as the ROS IP Address in the RosConnect in Unity, and that the `src/niryo_moveit/config/params.yaml` values are set correctly. - If the ROS TCP handshake fails (e.g. `ROS-Unity server listening...` printed on the Unity side but no `ROS-Unity Handshake received` on the ROS side), the ROS IP may not have been set correctly in the params.yaml file. Try running `echo "ROS_IP: $(hostname -I)" > src/niryo_moveit/config/params.yaml` in a terminal from your ROS workspace. @@ -218,7 +218,7 @@ ROS and Unity have now successfully connected! - [TCP Endpoint](https://github.com/Unity-Technologies/ROS-TCP-Endpoint) package - [Niryo One ROS stack](https://github.com/NiryoRobotics/niryo_one_ros) - [MoveIt Msgs](https://github.com/ros-planning/moveit_msgs) - + --- #### Proceed to [Part 3](3_pick_and_place.md). diff --git a/tutorials/pick_and_place/4_pick_and_place.md b/tutorials/pick_and_place/4_pick_and_place.md index 3eaa0bea..dad4e25f 100644 --- a/tutorials/pick_and_place/4_pick_and_place.md +++ b/tutorials/pick_and_place/4_pick_and_place.md @@ -16,7 +16,7 @@ This part is going to be a little different than the previous tutorials in that - [Add niryo_moveit Package](#add-niryo_moveit-package) - [Execution](#execution) - + # Niryo One Information The best source of information for the Niryo One is the [User Manual](https://niryo.com/docs/niryo-one/user-manual/complete-user-manual/). It contains a lot of general information about the Niryo One, such as how to connect it to a network, how to log in to it, and how to use the Niryo One Studio desktop application. @@ -115,7 +115,7 @@ command_list: ``` -From here we see that the `RobotMoveGoal.RobotMoveCommand.ToolCommand.cmd_type` variable will need to be `2` to close the gripper and `1` to open it. +From here we see that the `RobotMoveGoal.RobotMoveCommand.ToolCommand.cmd_type` variable will need to be `2` to close the gripper and `1` to open it. # Differences From Part 3 @@ -173,20 +173,20 @@ We will also make use of the `sim_and_real_pnp.py` script. It is very similar to ```python def send_trajectory_goal(client, trajectory): - + # Build the goal goal = RobotMoveGoal() goal.cmd.Trajectory = trajectory goal.cmd.cmd_type = TRAJECTORY_COMMAND_ID - + client.send_goal(goal) client.wait_for_result() - + return ``` - To send a gripper command: - + ```python def send_tool_goal(client, gripper_command): tool_command = ToolCommand() @@ -194,35 +194,35 @@ We will also make use of the `sim_and_real_pnp.py` script. It is very similar to tool_command.cmd_type = gripper_command tool_command.gripper_open_speed = GRIPPER_SPEED tool_command.gripper_close_speed = GRIPPER_SPEED - + goal = RobotMoveGoal() goal.cmd.tool_cmd = tool_command goal.cmd.cmd_type = TOOL_COMMAND_ID - + client.send_goal(goal) client.wait_for_result() - + return ``` - The `pick_and_place` function has been updated to call the two previous functions instead of appending the trajectory or gripper command to a list. - Example from `mover.py` - + ```python previous_ending_joint_angles = grasp_pose.joint_trajectory.points[-1].positions response.trajectories.append(grasp_pose) ``` - + - Updated code in `sim_real_pnp.py` - + ```python previous_ending_joint_angles = grasp_pose.trajectory.joint_trajectory.points[-1].positions send_trajectory_goal(client, grasp_pose) ``` # The Unity Side -Using the same scene from [Part 3](3_pick_and_place.md), we are going to use a new script, `RealSimPickAndPlace.cs`, that mirrors a lot of the functionality of the `TrajectoryPlanner.cs` script. +Using the same scene from [Part 3](3_pick_and_place.md), we are going to use a new script, `RealSimPickAndPlace.cs`, that mirrors a lot of the functionality of the `TrajectoryPlanner.cs` script. ## Key Differences @@ -272,7 +272,7 @@ void Start() { var jointPositions = point.positions; float[] result = jointPositions.Select(r=> (float)r * Mathf.Rad2Deg).ToArray(); - + // Set the joint values for every joint for (int joint = 0; joint < jointArticulationBodies.Length; joint++) { @@ -303,9 +303,9 @@ void Start() 1. Select the Publisher GameObject and add the `RealSimPickAndPlace` script as a component. -1. Note that the RealSimPickAndPlace component shows its member variables in the Inspector window, which need to be assigned. +1. Note that the RealSimPickAndPlace component shows its member variables in the Inspector window, which need to be assigned. - Once again, drag and drop the `Target` and `TargetPlacement` objects onto the Target and Target Placement Inspector fields, respectively. Assign the `niryo_one` robot to the Niryo One field. + Once again, drag and drop the `Target` and `TargetPlacement` objects onto the Target and Target Placement Inspector fields, respectively. Assign the `niryo_one` robot to the Niryo One field. ![](img/4_script.png) @@ -325,7 +325,7 @@ void Start() - The two files that will need to be updated are `niryo_one.urdf.xacro` and `without_mesh_niryo_one.urdf.xacro` located in the `/home/niryo/catkin_ws/src/niryo_one_description/urdf/v2` directory. - Look for the joint named `joint_world` and update the `origin`'s `xyz` to `0 0 0.63` to reflect that the simulated Niryo is placed at `0.63` on the Z axis. - + ```xml diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/PickAndPlaceMessageGenerationtests.cs b/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/PickAndPlaceMessageGenerationtests.cs index cfa57f8f..09500a63 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/PickAndPlaceMessageGenerationtests.cs +++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/Tests/EditMode/PickAndPlaceMessageGenerationtests.cs @@ -18,7 +18,7 @@ static IEnumerable IndividualMessages() { yield return new TestCaseData(Path.Combine(k_ROSDirectory, "moveit_msgs", "msg", "RobotTrajectory.msg")); } - + // Define directories of message files to be generated here static IEnumerable MessageDirectories() { @@ -46,7 +46,7 @@ public void TestMessageBuildDirectory_ThrowsNoExceptions(string directoryToBuild MessageAutoGen.GenerateDirectoryMessages(directoryToBuild, m_MessageGenOutputPath); AssetDatabase.Refresh(); } - + [Test] [TestCaseSource(nameof(ServiceDirectories))] public void TestServiceBuildDirectory_ThrowsNoExceptions(string directoryToBuild) diff --git a/tutorials/pick_and_place/README.md b/tutorials/pick_and_place/README.md index 61f60818..3e5edd77 100644 --- a/tutorials/pick_and_place/README.md +++ b/tutorials/pick_and_place/README.md @@ -2,7 +2,7 @@ # Pick-and-Place Tutorial -Unity's tools for robotic simulation enable users to integrate Unity with ROS-based workflows. [ROS](http://wiki.ros.org/ROS/Introduction) (Robot Operating System) provides services such as message-passing, package management, low-level device control, and hardware abstraction. Unity's robotics tools are able to support **importing URDF files** and **sending and receiving messages between ROS and Unity**. This tutorial will go through the steps necessary to integrate ROS with Unity, from installing the Unity Editor to creating a scene with an imported URDF to completing a pick-and-place task with known poses using [MoveIt](https://moveit.ros.org/) trajectory planning. +Unity's tools for robotic simulation enable users to integrate Unity with ROS-based workflows. [ROS](http://wiki.ros.org/ROS/Introduction) (Robot Operating System) provides services such as message-passing, package management, low-level device control, and hardware abstraction. Unity's robotics tools are able to support **importing URDF files** and **sending and receiving messages between ROS and Unity**. This tutorial will go through the steps necessary to integrate ROS with Unity, from installing the Unity Editor to creating a scene with an imported URDF to completing a pick-and-place task with known poses using [MoveIt](https://moveit.ros.org/) trajectory planning. This tutorial is designed such that you do not need prior experience with Unity or C# in order to follow the scene setup steps, and you do not need prior robotics experience to get started with ROS integration. The tutorial is divided into high-level phases, from basic Unity and ROS initial setup through executing a pick-and-place task. @@ -31,11 +31,11 @@ This repository provides project files for the pick-and-place tutorial, includin This part provides two options for setting up your ROS workspace: using Docker, or manually setting up a catkin workspace. -## [Part 1: Create Unity scene with imported URDF](1_urdf.md) +## [Part 1: Create Unity scene with imported URDF](1_urdf.md) -This part includes downloading and installing the Unity Editor, setting up a basic Unity scene, and importing a robot--the [Niryo One](https://niryo.com/niryo-one/)--using the URDF Importer. +This part includes downloading and installing the Unity Editor, setting up a basic Unity scene, and importing a robot--the [Niryo One](https://niryo.com/niryo-one/)--using the URDF Importer. ## [Part 2: ROS–Unity Integration](2_ros_tcp.md) @@ -44,7 +44,7 @@ This part includes downloading and installing the Unity Editor, setting up a bas This part covers creating a TCP connection between Unity and ROS, generating C# scripts from a ROS msg and srv files, and publishing to a ROS topic. ## [Part 3: Pick-and-Place In Unity](3_pick_and_place.md) - + This part includes the preparation and setup necessary to run a pick-and-place task with known poses using MoveIt. Steps covered include creating and invoking a motion planning service in ROS, moving a Unity Articulation Body based on a calculated trajectory, and controlling a gripping tool to successfully grasp and drop an object. diff --git a/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs b/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs index c5e0a6b2..7fc2fab5 100644 --- a/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs +++ b/tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs @@ -8,20 +8,20 @@ public class SourceDestinationPublisher : MonoBehaviour { // ROS Connector private ROSConnection ros; - + // Variables required for ROS communication public string topicName = "SourceDestination_input"; public GameObject niryoOne; public GameObject target; public GameObject targetPlacement; - + private int numRobotJoints = 6; private readonly Quaternion pickOrientation = Quaternion.Euler(90, 90, 0); - + // Articulation Bodies private ArticulationBody[] jointArticulationBodies; - + /// /// /// @@ -36,16 +36,16 @@ void Start() string arm_link = shoulder_link + "/arm_link"; jointArticulationBodies[1] = niryoOne.transform.Find(arm_link).GetComponent(); - + string elbow_link = arm_link + "/elbow_link"; jointArticulationBodies[2] = niryoOne.transform.Find(elbow_link).GetComponent(); - + string forearm_link = elbow_link + "/forearm_link"; jointArticulationBodies[3] = niryoOne.transform.Find(forearm_link).GetComponent(); - + string wrist_link = forearm_link + "/wrist_link"; jointArticulationBodies[4] = niryoOne.transform.Find(wrist_link).GetComponent(); - + string hand_link = wrist_link + "/hand_link"; jointArticulationBodies[5] = niryoOne.transform.Find(hand_link).GetComponent(); } diff --git a/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs b/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs index ead3e41b..a9b57d2e 100644 --- a/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs +++ b/tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs @@ -16,7 +16,7 @@ public class TrajectoryPlanner : MonoBehaviour private readonly float jointAssignmentWait = 0.1f; private readonly float poseAssignmentWait = 0.5f; private readonly Vector3 pickPoseOffset = Vector3.up * 0.1f; - + // Assures that the gripper is always positioned above the target cube before grasping. private readonly Quaternion pickOrientation = Quaternion.Euler(90, 90, 0); @@ -43,7 +43,7 @@ private enum Poses PickUp, Place }; - + /// /// Close the gripper /// @@ -81,7 +81,7 @@ private void OpenGripper() MNiryoMoveitJoints CurrentJointConfig() { MNiryoMoveitJoints joints = new MNiryoMoveitJoints(); - + joints.joint_00 = jointArticulationBodies[0].xDrive.target; joints.joint_01 = jointArticulationBodies[1].xDrive.target; joints.joint_02 = jointArticulationBodies[2].xDrive.target; @@ -103,7 +103,7 @@ public void PublishJoints() { MMoverServiceRequest request = new MMoverServiceRequest(); request.joints_input = CurrentJointConfig(); - + // Pick Pose request.pick_pose = new MPose { @@ -154,18 +154,18 @@ private IEnumerator ExecuteTrajectories(MMoverServiceResponse response) if (response.trajectories != null) { // For every trajectory plan returned - for (int poseIndex = 0 ; poseIndex < response.trajectories.Length; poseIndex++) + for (int poseIndex = 0; poseIndex < response.trajectories.Length; poseIndex++) { // For every robot pose in trajectory plan - for (int jointConfigIndex = 0 ; jointConfigIndex < response.trajectories[poseIndex].joint_trajectory.points.Length; jointConfigIndex++) + for (int jointConfigIndex = 0; jointConfigIndex < response.trajectories[poseIndex].joint_trajectory.points.Length; jointConfigIndex++) { var jointPositions = response.trajectories[poseIndex].joint_trajectory.points[jointConfigIndex].positions; - float[] result = jointPositions.Select(r=> (float)r * Mathf.Rad2Deg).ToArray(); - + float[] result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray(); + // Set the joint values for every joint for (int joint = 0; joint < jointArticulationBodies.Length; joint++) { - var joint1XDrive = jointArticulationBodies[joint].xDrive; + var joint1XDrive = jointArticulationBodies[joint].xDrive; joint1XDrive.target = result[joint]; jointArticulationBodies[joint].xDrive = joint1XDrive; } @@ -176,7 +176,7 @@ private IEnumerator ExecuteTrajectories(MMoverServiceResponse response) // Close the gripper if completed executing the trajectory for the Grasp pose if (poseIndex == (int)Poses.Grasp) CloseGripper(); - + // Wait for the robot to achieve the final pose from joint assignment yield return new WaitForSeconds(poseAssignmentWait); } @@ -200,16 +200,16 @@ void Start() string arm_link = shoulder_link + "/arm_link"; jointArticulationBodies[1] = niryoOne.transform.Find(arm_link).GetComponent(); - + string elbow_link = arm_link + "/elbow_link"; jointArticulationBodies[2] = niryoOne.transform.Find(elbow_link).GetComponent(); - + string forearm_link = elbow_link + "/forearm_link"; jointArticulationBodies[3] = niryoOne.transform.Find(forearm_link).GetComponent(); - + string wrist_link = forearm_link + "/wrist_link"; jointArticulationBodies[4] = niryoOne.transform.Find(wrist_link).GetComponent(); - + string hand_link = wrist_link + "/hand_link"; jointArticulationBodies[5] = niryoOne.transform.Find(hand_link).GetComponent(); @@ -225,4 +225,4 @@ void Start() rightGripper = rightGripperGameObject.GetComponent(); leftGripper = leftGripperGameObject.GetComponent(); } -} \ No newline at end of file +} diff --git a/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs b/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs index 724f710d..a02eef5a 100644 --- a/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs +++ b/tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs @@ -18,7 +18,7 @@ public class RealSimPickAndPlace : MonoBehaviour private ROSConnection ros; private const int NUM_ROBOT_JOINTS = 6; - // Hardcoded variables + // Hardcoded variables private const float JOINT_ASSIGNMENT_WAIT = 0.038f; private readonly Vector3 PICK_POSE_OFFSET = Vector3.up * 0.15f; @@ -151,7 +151,7 @@ void Start() /// /// Execute robot commands receved from ROS Subscriber. - /// Gripper commands will be executed immeditately wihle trajectories will be + /// Gripper commands will be executed immeditately wihle trajectories will be /// executed in a coroutine. /// /// RobotMoveActionGoal of trajectory or gripper commands @@ -178,10 +178,10 @@ void ExecuteRobotCommands(MRobotMoveActionGoal robotAction) /// /// Execute trajectories from RobotMoveActionGoal topic. - /// - /// Execution will iterate through every robot pose in the trajectory pose + /// + /// Execution will iterate through every robot pose in the trajectory pose /// array while updating the joint values on the simulated robot. - /// + /// /// /// The array of poses for the robot to execute private IEnumerator ExecuteTrajectories(MRobotTrajectory trajectories) @@ -190,12 +190,12 @@ private IEnumerator ExecuteTrajectories(MRobotTrajectory trajectories) foreach (var point in trajectories.joint_trajectory.points) { var jointPositions = point.positions; - float[] result = jointPositions.Select(r=> (float)r * Mathf.Rad2Deg).ToArray(); + float[] result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray(); // Set the joint values for every joint for (int joint = 0; joint < jointArticulationBodies.Length; joint++) { - var joint1XDrive = jointArticulationBodies[joint].xDrive; + var joint1XDrive = jointArticulationBodies[joint].xDrive; joint1XDrive.target = result[joint]; jointArticulationBodies[joint].xDrive = joint1XDrive; } diff --git a/tutorials/quick_setup.md b/tutorials/quick_setup.md index 2945b708..2c714958 100644 --- a/tutorials/quick_setup.md +++ b/tutorials/quick_setup.md @@ -2,12 +2,12 @@ This page provides brief instructions on installing the Unity Robotics packages. Head over to the [Pick-and-Place Tutorial](pick_and_place/README.md) for more detailed instructions and steps for building a sample project. -1. Create or open a Unity project. +1. Create or open a Unity project. > Note: If you are adding the URDF-Importer, ensure you are using a [2020.2.0](https://unity3d.com/unity/whats-new/2020.2.0)+ version of Unity Editor. 1. Open `Window` -> `Package Manager`. -1. In the Package Manager window, find and click the `+` button in the upper lefthand corner of the window. Select `Add package from git URL...`. +1. In the Package Manager window, find and click the `+` button in the upper lefthand corner of the window. Select `Add package from git URL...`. ![](../images/packman.png) @@ -15,6 +15,6 @@ This page provides brief instructions on installing the Unity Robotics packages. 1. For the [ROS-TCP-Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector), enter `https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector`. 1. For the [URDF-Importer](https://github.com/Unity-Technologies/URDF-Importer), enter `https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer`. -1. Click `Add`. +1. Click `Add`. To install from a local clone of the repository, see [installing a local package](https://docs.unity3d.com/Manual/upm-ui-local.html) in the Unity manual. diff --git a/tutorials/ros_unity_integration/network.md b/tutorials/ros_unity_integration/network.md index 5625199c..cefb66ed 100644 --- a/tutorials/ros_unity_integration/network.md +++ b/tutorials/ros_unity_integration/network.md @@ -30,7 +30,7 @@ The minimum settings required for Unity to communicate with ROS is to set the `R ## If Using Docker -The container will need to be started with the following arguments to forward the ports used for communication between ROS and Unity. +The container will need to be started with the following arguments to forward the ports used for communication between ROS and Unity. `-p 10000:10000 -p 5005:5005` diff --git a/tutorials/ros_unity_integration/publisher.md b/tutorials/ros_unity_integration/publisher.md index 14ef0e7b..502d1ab2 100644 --- a/tutorials/ros_unity_integration/publisher.md +++ b/tutorials/ros_unity_integration/publisher.md @@ -9,7 +9,7 @@ Create a simple Unity scene which publishes a GameObject's position and rotation - Follow the [ROS–Unity Initial Setup](setup.md) guide. - Open a new terminal window and run the following commands: - + ```bash source devel/setup.bash rosrun robotics_demo server_endpoint.py @@ -42,21 +42,21 @@ using UnityEngine; using Unity.Robotics.ROSTCPConnector; /// -/// +/// /// public class RosPublisherExample : MonoBehaviour { ROSConnection ros; public string topicName = "pos_rot"; - // The game object + // The game object public GameObject cube; // Publish the cube's position and rotation every N seconds public float publishMessageFrequency = 0.5f; // Used to determine how much time has elapsed since the last message was published private float timeElapsed; - + void Start() { // start the ROS connection diff --git a/tutorials/ros_unity_integration/server_endpoint.md b/tutorials/ros_unity_integration/server_endpoint.md index aa65fdc6..a27fc7e6 100644 --- a/tutorials/ros_unity_integration/server_endpoint.md +++ b/tutorials/ros_unity_integration/server_endpoint.md @@ -24,14 +24,14 @@ def main(): connections = rospy.get_param("/TCP_CONNECTIONS", 10) tcp_server = TcpServer(ros_node_name, buffer_size, connections) rospy.init_node(ros_node_name, anonymous=True) - + tcp_server.start({ 'pos_rot': RosPublisher('pos_rot', PosRot, queue_size=10), 'color': RosSubscriber('color', UnityColor, tcp_server), 'pos_srv': RosService('pos_srv', PositionService), 'obj_pose_srv': UnityService('obj_pose_srv', ObjectPoseService, tcp_server), }) - + rospy.spin() @@ -74,7 +74,7 @@ The `ros_node_name` argument is required and the `buffer_size` and `connections` 'pos_srv': RosService('pos_srv', PositionService), 'obj_pose_srv': UnityService('obj_pose_srv', ObjectPoseService, tcp_server), }) - + rospy.spin() ``` @@ -141,7 +141,7 @@ An example launch file that will set the appropriate ROSPARAM values required fo - + diff --git a/tutorials/ros_unity_integration/subscriber.md b/tutorials/ros_unity_integration/subscriber.md index 6f0cd488..5ae578d6 100644 --- a/tutorials/ros_unity_integration/subscriber.md +++ b/tutorials/ros_unity_integration/subscriber.md @@ -11,7 +11,7 @@ Create a simple Unity scene which subscribes to a [ROS topic](http://wiki.ros.or - Follow the [ROS–Unity Initial Setup](setup.md) guide. - Open a new terminal window, navigate to your Catkin workspace, and run the following commands: - + ```bash source devel/setup.bash rosrun robotics_demo server_endpoint.py @@ -21,7 +21,7 @@ Once the server_endpoint has started, it will print something similar to `[INFO] - In Unity, we need to generate the C# code for the `UnityColor` message. Open `Robotics` -> `Generate ROS Messages...`. - Set the ROS message path to `PATH/TO/Unity-Robotics-Hub/tutorials/ros_packages/robotics_demo/`, expand the robotics_demo subfolder and click `Build 2 msgs`. - + ![](images/generate_messages_1.png) - The generated files will be saved in the default directory `Assets/RosMessages/RoboticsDemo/msg`. diff --git a/tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs b/tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs index 3a715a0b..d2e419b8 100644 --- a/tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs +++ b/tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs @@ -47,7 +47,7 @@ private void Update() // Send message to ROS and return the response ros.SendServiceMessage(serviceName, positionServiceRequest, Callback_Destination); - awaitingResponseUntilTimestamp = Time.time+1.0f; // don't send again for 1 second, or until we receive a response + awaitingResponseUntilTimestamp = Time.time + 1.0f; // don't send again for 1 second, or until we receive a response } } @@ -57,4 +57,4 @@ void Callback_Destination(MPositionServiceResponse response) destination = new Vector3(response.output.pos_x, response.output.pos_y, response.output.pos_z); Debug.Log("New Destination: " + destination); } -} \ No newline at end of file +} diff --git a/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs b/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs index 6b3a69a6..c36a3da3 100644 --- a/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs +++ b/tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs @@ -1,18 +1,18 @@ -using UnityEngine; -using Unity.Robotics.ROSTCPConnector; -using RosColor = RosMessageTypes.RoboticsDemo.MUnityColor; - -public class RosSubscriberExample : MonoBehaviour -{ - public GameObject cube; - - void Start() - { - ROSConnection.instance.Subscribe("color", ColorChange); - } - - void ColorChange(RosColor colorMessage) - { - cube.GetComponent().material.color = new Color32((byte)colorMessage.r, (byte)colorMessage.g, (byte)colorMessage.b, (byte)colorMessage.a); - } -} +using UnityEngine; +using Unity.Robotics.ROSTCPConnector; +using RosColor = RosMessageTypes.RoboticsDemo.MUnityColor; + +public class RosSubscriberExample : MonoBehaviour +{ + public GameObject cube; + + void Start() + { + ROSConnection.instance.Subscribe("color", ColorChange); + } + + void ColorChange(RosColor colorMessage) + { + cube.GetComponent().material.color = new Color32((byte)colorMessage.r, (byte)colorMessage.g, (byte)colorMessage.b, (byte)colorMessage.a); + } +} diff --git a/tutorials/ros_unity_integration/unity_service.md b/tutorials/ros_unity_integration/unity_service.md index eb61af4f..15235896 100644 --- a/tutorials/ros_unity_integration/unity_service.md +++ b/tutorials/ros_unity_integration/unity_service.md @@ -11,7 +11,7 @@ Create a simple Unity scene which create a [Service](http://wiki.ros.org/Service - Follow the [ROS–Unity Initial Setup](setup.md) guide. - Open a new terminal window, navigate to your ROS workspace, and run the following commands: - + ```bash source devel/setup.bash rosrun robotics_demo server_endpoint.py @@ -26,7 +26,7 @@ Once the server_endpoint has started, it will print something similar to `[INFO] ![](images/generate_messages_2.png) - The generated files will be saved in the default directory `Assets/RosMessages/RoboticsDemo/srv`. - + - Create a new C# script and name it `RosUnityServiceExample.cs` - Paste the following code into `RosUnityServiceExample.cs` - **Note:** This script can be found at `tutorials/ros_unity_integration/unity_scripts`. @@ -65,13 +65,13 @@ public class RosUnityServiceExample : MonoBehaviour MObjectPoseServiceResponse objectPoseResponse = new MObjectPoseServiceResponse(); // Find a game object with the requested name GameObject gameObject = GameObject.Find(request.object_name); - if (gameObject) + if (gameObject) { // Fill-in the response with the object pose converted from Unity coordinate to ROS coordinate system objectPoseResponse.object_pose.position = gameObject.transform.position.To(); objectPoseResponse.object_pose.orientation = gameObject.transform.rotation.To(); } - + return objectPoseResponse; } } @@ -79,7 +79,7 @@ public class RosUnityServiceExample : MonoBehaviour - From the main menu bar, open `Robotics/ROS Settings`, and change the `ROS IP Address` variable to the ROS IP. - Create an empty GameObject and name it `UnityService`. -- Attach the `RosUnityServiceExample` script to the `UnityService` GameObject. +- Attach the `RosUnityServiceExample` script to the `UnityService` GameObject. - Pressing play in the Editor should start running as a ROS node, waiting to accept ObjectPose requests. Once a connection to ROS has been established, a message will be printed on the ROS terminal similar to `Connection from 172.17.0.1`. @@ -94,12 +94,12 @@ public class RosUnityServiceExample : MonoBehaviour ```bash Requesting pose for Cube - Pose for Cube: - position: + Pose for Cube: + position: x: 0.0 y: -1.0 z: 0.20000000298023224 - orientation: + orientation: x: 0.0 y: -0.0 z: 0.0 @@ -113,12 +113,12 @@ You may replace `Cube` with the name of any other GameObject currently present i rosservice call /obj_pose_srv Cube ``` ```bash - object_pose: - position: + object_pose: + position: x: 0.0 y: -1.0 z: 0.20000000298023224 - orientation: + orientation: x: 0.0 y: -0.0 z: 0.0 diff --git a/tutorials/urdf_importer/urdf_appendix.md b/tutorials/urdf_importer/urdf_appendix.md index 4c6129d7..3c8b7c5a 100644 --- a/tutorials/urdf_importer/urdf_appendix.md +++ b/tutorials/urdf_importer/urdf_appendix.md @@ -3,9 +3,9 @@ ## File Hierarchy URDF files and associated meshes should be placed in a single folder within the Assets directory of Unity. We suggest creating a new folder with the robot's name and place the URDF file in its root with all associated mesh files in sub folders. Be sure to update the file locations as described by the URDF file. -## GameObject Hierarchy +## GameObject Hierarchy -The robot imported in Unity follows a Parent - Child hierarchy. +The robot imported in Unity follows a Parent - Child hierarchy. - Robot GameObject - UrdfRobot Script: UrdfRobot.cs script contains functions to change the behaviors of the imported robot. The script is used to control a robot's behavior, compare against a URDF file, and export the robot as a URDF file. - Controller Script: Enables keyboard control of the robot. It can be modified to create custom controllers as described [here](##Guide-to-write-your-own-controller). @@ -30,12 +30,12 @@ URDF comparator is a testing tool to compare two URDF files. This tool can be us - After the exported file location is selected, its location is automatically selected as location of `Log File Save Location`. You can choose your own save location by pressing the `Select` next to the text field. - To compare the two URDF files press `Compare URDF Files` to generate the [log file](#URDF_Comparator). -## URDF_Comparator +## URDF_Comparator The URDF comparator generates a log file which details the results of the comparison. Log file contains two types of log blocks. One type of block details joint: ``` ********LINK***** - Name: + Name: Equal: True Name: Source: base_link Exported: base_link Inertial Checks @@ -64,10 +64,10 @@ The URDF comparator generates a log file which details the results of the compar Material Checks Material Nullity Check: True Name Equal: True - Name: Source: LightGrey + Name: Source: LightGrey Exported: LightGrey Colors Equal: True - RGB :0.700 0.700 0.700 + RGB :0.700 0.700 0.700 Texture nullity equality: True Collisions Checks Number of Collision Components @@ -77,8 +77,8 @@ Exported Count: 0001 -Collision Component: 1 Collision Name Equal: True - Name: Source: - Exported: + Name: Source: + Exported: Origin Checks Origin Nullity Check: True Geometry: @@ -118,9 +118,9 @@ Parent: Exported: base_link Axis Checks Axis Equal: True - XYZ : (0.000,0.000,1.000) + XYZ : (0.000,0.000,1.000) Dynamics Checks - Dynamics Equal: True + Dynamics Equal: True Lower Limit: Equal: True Lower Limit Value: -3.14159265359 @@ -135,7 +135,7 @@ Child Name: Source: shoulder_link Child Name: Exported: shoulder_link ``` -Log file contains comparison for every attribute contained in the source URDF file to the corresponding attribute in exported URDF file. Each attribute follows the format : +Log file contains comparison for every attribute contained in the source URDF file to the corresponding attribute in exported URDF file. Each attribute follows the format : ``` Equal : @@ -144,7 +144,7 @@ Equal : ``` Some attributes generates a nullity check which checks for presence of attributes in both URDF files. -## Articulation Body axis definition +## Articulation Body axis definition ![](images/Unity_coordinate.png) The most commonly used Coordinate system is the right hand coordinate system which is shown in the figure above. It follows the convention that if your right-hand thumb represents the X direction, then the index finger and the middle finger stretched out uncrossed at right angles would represent the Y and Z direction respectively. Positive direction of rotation is represented by the curl of the fingers with the thumb stretched out. @@ -163,7 +163,7 @@ Note: When the mesh is imported in Unity, the x axis is negated to convert the m Articulation Body allows the joints to be controlled by three methods: 1. Position Control -2. Torque Control +2. Torque Control 3. Velocity Control All types of control are governed by the Spring-Damper equation : @@ -174,7 +174,7 @@ F = stiffness * (currentPosition - target) - damping * (currentVelocity - target #### Example Code -Code for positional control can be divided into two scripts. One script would be attached to the root GameObject which represents the robot. This script would determine the correct position of the joint, directed by a control policy implemented by the user. The other end of the script is attached to the GameObject with ArticulationBody component which receives the target position and assigns that +Code for positional control can be divided into two scripts. One script would be attached to the root GameObject which represents the robot. This script would determine the correct position of the joint, directed by a control policy implemented by the user. The other end of the script is attached to the GameObject with ArticulationBody component which receives the target position and assigns that value to the articulation body. **Note: This architecture can be changed according to the user's design.** @@ -215,7 +215,7 @@ void Update() } Highlight(selectedIndex); } - + UpdateDirection(selectedIndex); } ``` @@ -224,10 +224,10 @@ The up and down arrow keys are used to update the direction in which the joint i private void UpdateDirection(int jointIndex) { float moveDirection = Input.GetAxis("Vertical"); - JointControl current = articulationChain[jointIndex].GetComponent(); + JointControl current = articulationChain[jointIndex].GetComponent(); if (previousIndex != jointIndex) { - JointControl previous = articulationChain[previousIndex].GetComponent(); + JointControl previous = articulationChain[previousIndex].GetComponent(); previous.direction = RotationDirection.None; previousIndex = jointIndex; } @@ -269,16 +269,16 @@ The joint control script determines the direction in which the joint needs to mo float newTargetDelta = (int)direction * Time.fixedDeltaTime * speed; if (newTargetDelta + currentDrive.target <= currentDrive.upperLimit && newTargetDelta + currentDrive.target >= currentDrive.lowerLimit){ currentDrive.target += newTargetDelta; - } + } joint.xDrive = currentDrive; - } + } } } } ``` -### Position Control +### Position Control Position Control can be imagined as adding a spring to a joint with linear spring in case of prismatic joint and a torsional spring in case of revolute joint. The joint follows Hooke's law: @@ -320,7 +320,7 @@ To address this predicament we have integrated another algorithm to create Conve URDF defines the robot visually using Visual Meshes and its inertial volume using collision meshes. Inertial meshes used to define the physical volume of the links and help in calculating the inertia of the links and detecting the collisions between different physical objects. When a collider mesh is imported in Unity, it is decomposed into near convex shapes to form a concave hull. This is necessary in detecting collisions between two mesh colliders. The changed shape might intersect with each other creating a hindrance in robot movement. To remedy this, we support a ```disable collision``` tag in URDF. To add an exception for collision detection in Unity: 1. Identify the links between which you want to ignore the collisions. -2. Add a tag in the URDF file with the format +2. Add a tag in the URDF file with the format ```XML link2=> diff --git a/tutorials/urdf_importer/urdf_tutorial.md b/tutorials/urdf_importer/urdf_tutorial.md index 966610f5..e4935c89 100644 --- a/tutorials/urdf_importer/urdf_tutorial.md +++ b/tutorials/urdf_importer/urdf_tutorial.md @@ -6,20 +6,20 @@ - Niryo One URDF files from [Niryo One ROS](https://github.com/NiryoRobotics/niryo_one_ros) - Working ROS environment -## Setting up the URDF Importer in Unity Editor +## Setting up the URDF Importer in Unity Editor - Integrate the URDF Importer following these [instructions](https://github.com/Unity-Technologies/URDF-Importer#integrate-urdf-importer-into-unity-project) - Create a new directory in `Assets` and name it `URDF` - Clone the [Niryo One ROS](https://github.com/NiryoRobotics/niryo_one_ros) repo and copy the `niryo_one_description` directory into `Assets/URDF` - This directory only includes a `.urdf.xacro` file which will need to be converted into a `.urdf` file before we can import it - Run the following command to convert Xacro to URDF, `rosrun xacro xacro --inorder -o PATH/TO/niryo_one.urdf PATH/TO/niryo_one.urdf.xacro` - Copy the generated `niryo_one.urdf` file to `Assets/URDF` - - Right click on the this file and select `Import Robot from URDF` - - Select the co-ordinate system in which the meshes were designed. Default mesh orientation is Y-up which is supported by Unity but some packages often use Z-up and X-up configuration. For more [information](https://docs.unity3d.com/Manual/HOWTO-FixZAxisIsUp.html). + - Right click on the this file and select `Import Robot from URDF` + - Select the co-ordinate system in which the meshes were designed. Default mesh orientation is Y-up which is supported by Unity but some packages often use Z-up and X-up configuration. For more [information](https://docs.unity3d.com/Manual/HOWTO-FixZAxisIsUp.html). - Select the Convex Mesh Decomposer you want to use for the imported robot. More information can be found [here](urdf_appendix.md##Convex-Mesh-Collider). - Click `Import` - + ## Using the Controller -- To add the controller to an imported robot click the `Enable` button in the Inspector window in front of the `Controller Script` option. This will add a Controller Script, FKrobot and Joint Control at runtime. +- To add the controller to an imported robot click the `Enable` button in the Inspector window in front of the `Controller Script` option. This will add a Controller Script, FKrobot and Joint Control at runtime. - To prevent the joints from slipping set the `Stiffness` and `Damping` to `100,000` and `10,000` respectively. - To be able to apply forces to the joints set the `Force Limit` to `10,000`. - To prevent the robot from falling over, in the GameObject tree expand `niryo_one` -> `world` -> `base_link` and set the toggle for `Immovable` for the base_link. From dc34c49213da2db30bea0ec4a78da9dd5ccfd250 Mon Sep 17 00:00:00 2001 From: peifeng-unity <56408141+peifeng-unity@users.noreply.github.com> Date: Mon, 22 Feb 2021 22:21:46 +0000 Subject: [PATCH 28/35] Fixing issue with Demo.cs not working with new URDF Importer co-routine (#162) Co-authored-by: Devin Miller --- .../PickAndPlaceProject/Assets/DemoScripts/Demo.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs index 0b4a591b..348cb0c6 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs +++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs @@ -9,6 +9,7 @@ using RosSharp; using RosSharp.Control; using RosSharp.Urdf.Editor; +using Unity.EditorCoroutines.Editor; using UnityEditor; using UnityEngine; using Unity.Robotics.ROSTCPConnector; From c139d6e922f31ce41b007e61fb3cd8b33db53e67 Mon Sep 17 00:00:00 2001 From: Amanda <31416491+at669@users.noreply.github.com> Date: Mon, 1 Mar 2021 22:00:10 -0700 Subject: [PATCH 29/35] Readme: New Link (#172) * Fixing issue with Demo.cs not working with new URDF Importer co-routine * remove unused import (#163) * README update * PR feedback * Blog post link update Co-authored-by: Devin Miller Co-authored-by: peifeng-unity --- README.md | 29 ++++++++++++++++++++++++----- 1 file changed, 24 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index fe327a8c..b5ea4f89 100644 --- a/README.md +++ b/README.md @@ -12,12 +12,26 @@ This is a central repository for tools, tutorials, resources, and documentation Simulation plays an important role in robotics development, and we’re here to ensure that roboticists can use Unity for these simulations. We're starting off with a set of tools to make it easier to use Unity with existing ROS-based workflows. Try out some of our samples below to get started quickly. -## Getting Started with Unity Robotics +## Getting Started +### [Quick Installation Instructions](tutorials/quick_setup.md) + +Brief steps on installing the Unity Robotics packages. + +### [Pick-and-Place Demo](tutorials/pick_and_place/README.md) + +A complete end-to-end demonstration, including how to set up the Unity environment, how to import a robot from URDF, and how to set up two-way communication with ROS for control. + +### [**New!**] [Object Pose Estimation Demo](https://github.com/Unity-Technologies/Robotics-Object-Pose-Estimation) + +A complete end-to-end demonstration in which we collect training data in Unity and use that data to train a deep neural network to predict the pose of a cube. This model is then deployed in a simulated robotic pick-and-place task. + +### [Articulations Robot Demo](https://github.com/Unity-Technologies/articulations-robot-demo) + +A robot simulation demonstrating Unity's new physics solver (no ROS dependency). +## Documentation | Tutorial | Description | |---|---| -| [Quick Installation Instructions](tutorials/quick_setup.md) | Brief steps on installing the Unity Robotics packages | -| [Pick-and-Place Demo](tutorials/pick_and_place/README.md) | A complete end-to-end demonstration, including how to set up the Unity environment, how to import a robot from URDF, and how to set up two-way communication with ROS for control | | [ROS–Unity Integration](tutorials/ros_unity_integration/README.md) | A set of component-level tutorials showing how to set up communication between ROS and Unity | | [URDF Importer](tutorials/urdf_importer/urdf_tutorial.md) | Steps on using the Unity package for loading [URDF](http://wiki.ros.org/urdf) files | | [Articulations Robot Demo](https://github.com/Unity-Technologies/articulations-robot-demo) | A robot simulation demonstrating Unity's new physics solver (no ROS dependency) @@ -27,8 +41,8 @@ Simulation plays an important role in robotics development, and we’re here to | Repo | Functionality | |---|---| -| [TCP Endpoint](https://github.com/Unity-Technologies/ROS-TCP-Endpoint) | ROS node for sending/receiving messages from Unity | -| [TCP Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector) | Unity package for sending/receiving messages from ROS | +| [ROS TCP Endpoint](https://github.com/Unity-Technologies/ROS-TCP-Endpoint) | ROS node for sending/receiving messages from Unity | +| [ROS TCP Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector) | Unity package for sending/receiving messages from ROS | | [URDF Importer](https://github.com/Unity-Technologies/URDF-Importer) | Unity package for loading [URDF](http://wiki.ros.org/urdf) files | @@ -37,6 +51,7 @@ Simulation plays an important role in robotics development, and we’re here to ### Blog Posts and Talks +- [**New!**] (March 2, 2021) Teaching robots to see with Unity [blog post](https://blogs.unity3d.com/2021/03/02/teaching-robots-to-see-with-unity/) - (November 19, 2020) Robotics simulation in Unity is as easy as 1, 2, 3! [blog post](https://blogs.unity3d.com/2020/11/19/robotics-simulation-in-unity-is-as-easy-as-1-2-3/) - (November 12, 2020) Unite Now 2020: Simulating Robots with ROS and Unity [video](https://resources.unity.com/unitenow/onlinesessions/simulating-robots-with-ros-and-unity) @@ -49,6 +64,7 @@ Use articulation bodies to easily prototype industrial designs with realistic mo - [Unity Industrial Simulation](https://unity.com/products/unity-simulation) - [Unity Computer Vision](https://unity.com/computer-vision) +- [Unity ML-Agents Toolkit](https://github.com/Unity-Technologies/ml-agents) ## New Physics Features in Unity ### New Features @@ -62,6 +78,9 @@ Here’s a peek into what our Physics Team is hard at work on… - **Force/Torque Sensor API**. This API will allow users to get the force and torque acting on an articulation body (useful for simulating a force/torque sensor!), as well as to get the motor torque applied by an articulation drive. - **Query primitives**. These simple, GameObject-less shapes allow for collision detection without requiring simulation (i.e., without calling Physics.Simulate). This feature will allow users to initialize objects in feasible locations, and can also be used for motion planning. +## [**New!**] ROS 2 +Interested in early access to ROS 2 integration? Email [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com) to join our alpha program. + ## FAQs [FAQs](faq.md) From ef695294a6e91aeb0cb55d08d71a1e8cfb615695 Mon Sep 17 00:00:00 2001 From: LaurieCheers-unity <73140792+LaurieCheers-unity@users.noreply.github.com> Date: Wed, 3 Mar 2021 10:09:32 -0800 Subject: [PATCH 30/35] Added newsletter link (#175) --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index b5ea4f89..e4ad3c48 100644 --- a/README.md +++ b/README.md @@ -99,5 +99,9 @@ For feature requests, bugs, or other issues, please file a [GitHub issue](https: For any other questions or feedback, connect directly with the Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). +## Newsletter + +To get notified about new updates and features, [sign up for our newsletter](https://create.unity3d.com/robotics-simulation-newsletter-sign-up)! + ## License [Apache License 2.0](LICENSE) From 29aaceea750a4acabe0f8837b4a63eec50778d4f Mon Sep 17 00:00:00 2001 From: peifeng-unity Date: Wed, 26 May 2021 14:20:48 -0700 Subject: [PATCH 31/35] remove stale merge --- README.md | 36 ++++--------------- .../Assets/DemoScripts/Demo.cs | 1 - 2 files changed, 6 insertions(+), 31 deletions(-) diff --git a/README.md b/README.md index e4ad3c48..d7dd2d6d 100644 --- a/README.md +++ b/README.md @@ -12,26 +12,12 @@ This is a central repository for tools, tutorials, resources, and documentation Simulation plays an important role in robotics development, and we’re here to ensure that roboticists can use Unity for these simulations. We're starting off with a set of tools to make it easier to use Unity with existing ROS-based workflows. Try out some of our samples below to get started quickly. -## Getting Started -### [Quick Installation Instructions](tutorials/quick_setup.md) - -Brief steps on installing the Unity Robotics packages. - -### [Pick-and-Place Demo](tutorials/pick_and_place/README.md) - -A complete end-to-end demonstration, including how to set up the Unity environment, how to import a robot from URDF, and how to set up two-way communication with ROS for control. - -### [**New!**] [Object Pose Estimation Demo](https://github.com/Unity-Technologies/Robotics-Object-Pose-Estimation) - -A complete end-to-end demonstration in which we collect training data in Unity and use that data to train a deep neural network to predict the pose of a cube. This model is then deployed in a simulated robotic pick-and-place task. - -### [Articulations Robot Demo](https://github.com/Unity-Technologies/articulations-robot-demo) - -A robot simulation demonstrating Unity's new physics solver (no ROS dependency). -## Documentation +## Getting Started with Unity Robotics | Tutorial | Description | |---|---| +| [Quick Installation Instructions](tutorials/quick_setup.md) | Brief steps on installing the Unity Robotics packages | +| [Pick-and-Place Demo](tutorials/pick_and_place/README.md) | A complete end-to-end demonstration, including how to set up the Unity environment, how to import a robot from URDF, and how to set up two-way communication with ROS for control | | [ROS–Unity Integration](tutorials/ros_unity_integration/README.md) | A set of component-level tutorials showing how to set up communication between ROS and Unity | | [URDF Importer](tutorials/urdf_importer/urdf_tutorial.md) | Steps on using the Unity package for loading [URDF](http://wiki.ros.org/urdf) files | | [Articulations Robot Demo](https://github.com/Unity-Technologies/articulations-robot-demo) | A robot simulation demonstrating Unity's new physics solver (no ROS dependency) @@ -41,8 +27,8 @@ A robot simulation demonstrating Unity's new physics solver (no ROS dependency). | Repo | Functionality | |---|---| -| [ROS TCP Endpoint](https://github.com/Unity-Technologies/ROS-TCP-Endpoint) | ROS node for sending/receiving messages from Unity | -| [ROS TCP Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector) | Unity package for sending/receiving messages from ROS | +| [TCP Endpoint](https://github.com/Unity-Technologies/ROS-TCP-Endpoint) | ROS node for sending/receiving messages from Unity | +| [TCP Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector) | Unity package for sending/receiving messages from ROS | | [URDF Importer](https://github.com/Unity-Technologies/URDF-Importer) | Unity package for loading [URDF](http://wiki.ros.org/urdf) files | @@ -51,7 +37,6 @@ A robot simulation demonstrating Unity's new physics solver (no ROS dependency). ### Blog Posts and Talks -- [**New!**] (March 2, 2021) Teaching robots to see with Unity [blog post](https://blogs.unity3d.com/2021/03/02/teaching-robots-to-see-with-unity/) - (November 19, 2020) Robotics simulation in Unity is as easy as 1, 2, 3! [blog post](https://blogs.unity3d.com/2020/11/19/robotics-simulation-in-unity-is-as-easy-as-1-2-3/) - (November 12, 2020) Unite Now 2020: Simulating Robots with ROS and Unity [video](https://resources.unity.com/unitenow/onlinesessions/simulating-robots-with-ros-and-unity) @@ -64,12 +49,6 @@ Use articulation bodies to easily prototype industrial designs with realistic mo - [Unity Industrial Simulation](https://unity.com/products/unity-simulation) - [Unity Computer Vision](https://unity.com/computer-vision) -- [Unity ML-Agents Toolkit](https://github.com/Unity-Technologies/ml-agents) - -## New Physics Features in Unity -### New Features -- **Contact Modification API** This API will allow users to define custom contact reactions, such as ignoring subsets of contact points, in order to help simulate holes, slippery surfaces, soft contacts, and more. It is available in Unity versions **2021.2a12+**. [Read more about the new Contact Modification API](https://forum.unity.com/threads/experimental-contacts-modification-api.924809/). -- **Collision detection modes exposed for ArticulationBody: discrete, sweep-based CCD, and speculative CCD**. New continuous collision detection (CCD) modes will ensure that fast-moving objects collide with objects, instead of tunneling or passing through those objects, which can happen in the default “discrete” mode. This API is available in Unity versions **2020.3.5f1+**. [Read more about continuous collision detection](https://docs.unity3d.com/2020.3/Documentation/ScriptReference/ArticulationBody-collisionDetectionMode.html). ### Coming Soon Here’s a peek into what our Physics Team is hard at work on… @@ -78,9 +57,6 @@ Here’s a peek into what our Physics Team is hard at work on… - **Force/Torque Sensor API**. This API will allow users to get the force and torque acting on an articulation body (useful for simulating a force/torque sensor!), as well as to get the motor torque applied by an articulation drive. - **Query primitives**. These simple, GameObject-less shapes allow for collision detection without requiring simulation (i.e., without calling Physics.Simulate). This feature will allow users to initialize objects in feasible locations, and can also be used for motion planning. -## [**New!**] ROS 2 -Interested in early access to ROS 2 integration? Email [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com) to join our alpha program. - ## FAQs [FAQs](faq.md) @@ -104,4 +80,4 @@ Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com) To get notified about new updates and features, [sign up for our newsletter](https://create.unity3d.com/robotics-simulation-newsletter-sign-up)! ## License -[Apache License 2.0](LICENSE) +[Apache License 2.0](LICENSE) \ No newline at end of file diff --git a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs index 348cb0c6..0b4a591b 100644 --- a/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs +++ b/tutorials/pick_and_place/PickAndPlaceProject/Assets/DemoScripts/Demo.cs @@ -9,7 +9,6 @@ using RosSharp; using RosSharp.Control; using RosSharp.Urdf.Editor; -using Unity.EditorCoroutines.Editor; using UnityEditor; using UnityEngine; using Unity.Robotics.ROSTCPConnector; From 5f780dffb40eac3c2918c85d76f3e8893268a265 Mon Sep 17 00:00:00 2001 From: Shuo Diao Date: Wed, 26 May 2021 16:30:39 -0700 Subject: [PATCH 32/35] Minor formmating --- tutorials/urdf_importer/urdf_appendix.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorials/urdf_importer/urdf_appendix.md b/tutorials/urdf_importer/urdf_appendix.md index 3c8b7c5a..9f918739 100644 --- a/tutorials/urdf_importer/urdf_appendix.md +++ b/tutorials/urdf_importer/urdf_appendix.md @@ -317,7 +317,7 @@ To address this predicament we have integrated another algorithm to create Conve ![](images/ConvexMeshComparison.png) ## Disable Collision Support -URDF defines the robot visually using Visual Meshes and its inertial volume using collision meshes. Inertial meshes used to define the physical volume of the links and help in calculating the inertia of the links and detecting the collisions between different physical objects. When a collider mesh is imported in Unity, it is decomposed into near convex shapes to form a concave hull. This is necessary in detecting collisions between two mesh colliders. The changed shape might intersect with each other creating a hindrance in robot movement. To remedy this, we support a ```disable collision``` tag in URDF. To add an exception for collision detection in Unity: +URDF defines the robot visually using Visual Meshes and its inertial volume using collision meshes. Inertial meshes used to define the physical volume of the links and help in calculating the inertia of the links and detecting the collisions between different physical objects. When a collider mesh is imported in Unity, it is decomposed into near convex shapes to form a concave hull. This is necessary in detecting collisions between two mesh colliders. The changed shape might intersect with each other creating a hindrance in robot movement. To remedy this, we support a ```disable collision``` tag in URDF. To add an exception for collision detection in Unity: 1. Identify the links between which you want to ignore the collisions. 2. Add a tag in the URDF file with the format From e405632f9619f993a6dc8dc9b7e46effa2e1f844 Mon Sep 17 00:00:00 2001 From: Shuo Diao Date: Wed, 26 May 2021 16:35:24 -0700 Subject: [PATCH 33/35] Fix README and git sub module --- .gitmodules | 3 +++ README.md | 11 ++++++----- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/.gitmodules b/.gitmodules index abd504f1..015a49a4 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "tutorials/pick_and_place/ROS/src/moveit_msgs"] path = tutorials/pick_and_place/ROS/src/moveit_msgs url = https://github.com/ros-planning/moveit_msgs.git +[submodule "tutorials/pick_and_place/ROS/src/ros_tcp_endpoint"] + path = tutorials/pick_and_place/ROS/src/ros_tcp_endpoint + url = https://github.com/Unity-Technologies/ROS-TCP-Endpoint.git diff --git a/README.md b/README.md index d7dd2d6d..fe327a8c 100644 --- a/README.md +++ b/README.md @@ -50,6 +50,11 @@ Use articulation bodies to easily prototype industrial designs with realistic mo - [Unity Industrial Simulation](https://unity.com/products/unity-simulation) - [Unity Computer Vision](https://unity.com/computer-vision) +## New Physics Features in Unity +### New Features +- **Contact Modification API** This API will allow users to define custom contact reactions, such as ignoring subsets of contact points, in order to help simulate holes, slippery surfaces, soft contacts, and more. It is available in Unity versions **2021.2a12+**. [Read more about the new Contact Modification API](https://forum.unity.com/threads/experimental-contacts-modification-api.924809/). +- **Collision detection modes exposed for ArticulationBody: discrete, sweep-based CCD, and speculative CCD**. New continuous collision detection (CCD) modes will ensure that fast-moving objects collide with objects, instead of tunneling or passing through those objects, which can happen in the default “discrete” mode. This API is available in Unity versions **2020.3.5f1+**. [Read more about continuous collision detection](https://docs.unity3d.com/2020.3/Documentation/ScriptReference/ArticulationBody-collisionDetectionMode.html). + ### Coming Soon Here’s a peek into what our Physics Team is hard at work on… @@ -75,9 +80,5 @@ For feature requests, bugs, or other issues, please file a [GitHub issue](https: For any other questions or feedback, connect directly with the Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). -## Newsletter - -To get notified about new updates and features, [sign up for our newsletter](https://create.unity3d.com/robotics-simulation-newsletter-sign-up)! - ## License -[Apache License 2.0](LICENSE) \ No newline at end of file +[Apache License 2.0](LICENSE) From 8b4e04e7b27cd4d98e9348880a438abf355bc30a Mon Sep 17 00:00:00 2001 From: Shuo Diao Date: Wed, 26 May 2021 16:38:06 -0700 Subject: [PATCH 34/35] Revert unintentional change in urdf_appendix.md --- tutorials/urdf_importer/urdf_appendix.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorials/urdf_importer/urdf_appendix.md b/tutorials/urdf_importer/urdf_appendix.md index 9f918739..9fd0e8c4 100644 --- a/tutorials/urdf_importer/urdf_appendix.md +++ b/tutorials/urdf_importer/urdf_appendix.md @@ -317,7 +317,7 @@ To address this predicament we have integrated another algorithm to create Conve ![](images/ConvexMeshComparison.png) ## Disable Collision Support -URDF defines the robot visually using Visual Meshes and its inertial volume using collision meshes. Inertial meshes used to define the physical volume of the links and help in calculating the inertia of the links and detecting the collisions between different physical objects. When a collider mesh is imported in Unity, it is decomposed into near convex shapes to form a concave hull. This is necessary in detecting collisions between two mesh colliders. The changed shape might intersect with each other creating a hindrance in robot movement. To remedy this, we support a ```disable collision``` tag in URDF. To add an exception for collision detection in Unity: +URDF defines the robot visually using Visual Meshes, and its collision using Collision Meshes. Collision meshes define the physical volume of the links, and are used to calculate the inertia of the links and also to detect collisions between different physical objects. In Unity, rigidbodies cannot have concave collision meshes, so when importing a concave collision mesh, all concave regions are closed over to produce a convex outline. As a result, the convex shapes might intersect with each other, creating a hindrance in robot movement. To remedy this, we support a ```disable collision``` tag in URDF. To add an exception for collision detection in Unity: 1. Identify the links between which you want to ignore the collisions. 2. Add a tag in the URDF file with the format From dd9136967c1aab8eb0be67d1d0be054c5a88d970 Mon Sep 17 00:00:00 2001 From: Shuo Diao Date: Wed, 26 May 2021 16:42:36 -0700 Subject: [PATCH 35/35] Add back tutorials/pick_and_place/ROS/src/ros_tcp_endpoint --- tutorials/pick_and_place/ROS/src/ros_tcp_endpoint | 1 + 1 file changed, 1 insertion(+) create mode 160000 tutorials/pick_and_place/ROS/src/ros_tcp_endpoint diff --git a/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint b/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint new file mode 160000 index 00000000..a5e0fd1c --- /dev/null +++ b/tutorials/pick_and_place/ROS/src/ros_tcp_endpoint @@ -0,0 +1 @@ +Subproject commit a5e0fd1ce1a711f557fe67e73657c74f92482fbe