11 KiB
Pick-and-Place Tutorial: Part 3
This part assumes you have access to a functional ROS workspace and that the previous two parts (Part 1, Part 2) have been completed.
Steps covered in this tutorial includes invoking a motion planning service in ROS, moving a Unity Articulation Body based on the calculated trajectory, and controlling a gripping tool to successfully grasp a cube.
Table of Contents
Part 3: Pick-and-Place
The Unity Side
-
If you have not already completed the steps in Part 1 to set up the Unity project and Part 2 to integrate ROS with Unity, do so now.
-
If the PickAndPlaceProject Unity project is not already open, select and open it from the Unity Hub.
Note the
Assets/Scripts/TrajectoryPlanner.cs
script. This is where all of the logic to invoke a motion planning service lives, as well as the logic to control the gripper end effector tool.The UI button
OnClick
callback will be reassigned later in this tutorial to the following function,PublishJoints
, as defined:public void PublishJoints() { MoverServiceRequest request = new MoverServiceRequest(); request.joints_input = CurrentJointConfig(); // Pick Pose request.pick_pose = new RosMessageTypes.Geometry.Pose { position = new Point( target.transform.position.z, -target.transform.position.x, // Add pick pose offset to position the gripper above target to avoid collisions target.transform.position.y + pickPoseOffset ), // Orientation is hardcoded for this example so the gripper is always directly above the target object orientation = pickOrientation }; // Place Pose request.place_pose = new RosMessageTypes.Geometry.Pose { position = new Point( targetPlacement.transform.position.z, -targetPlacement.transform.position.x, // Use the same pick pose offset so the target cube can be seen dropping into position targetPlacement.transform.position.y + pickPoseOffset ), // Orientation is hardcoded for this example so the gripper is always directly above the target object orientation = pickOrientation }; ros.SendServiceMessage<MoverServiceResponse>(rosServiceName, request, TrajectoryResponse); } void TrajectoryResponse(MoverServiceResponse response) { if (response.trajectories != null) { Debug.Log("Trajectory returned."); StartCoroutine(ExecuteTrajectories(response)); } else { Debug.LogError("No trajectory returned from MoverService."); } }
This is similar to the
SourceDestinationPublisher.Publish()
function, but with a few key differences. There is an addedpickPoseOffset
to thepick
andplace_pose
y
component. This is because the calculated trajectory to grasp thetarget
object will hover slightly above the object before grasping it in order to avoid potentially colliding with the object. Additionally, this function callsCurrentJointConfig()
to assign therequest.joints_input
instead of assigning the values individually.The
response.trajectories
are received in theTrajectoryResponse()
callback, as defined in theros.SendServiceMessage
parameters. These trajectories are passed toExecuteTrajectories()
below:private IEnumerator ExecuteTrajectories(MoverServiceResponse response) { if (response.trajectories != null) { for (int poseIndex = 0 ; poseIndex < response.trajectories.Length; poseIndex++) { 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(); for (int joint = 0; joint < jointArticulationBodies.Length; joint++) { var joint1XDrive = jointArticulationBodies[joint].xDrive; joint1XDrive.target = result[joint]; jointArticulationBodies[joint].xDrive = joint1XDrive; } yield return new WaitForSeconds(jointAssignmentWait); } if (poseIndex == (int)Poses.Grasp) CloseGripper(); yield return new WaitForSeconds(poseAssignmentWait); } // Open Gripper at end of sequence OpenGripper(); } }
ExecuteTrajectories
iterates through the joints to assign a newxDrive.target
value based on the ROS service response, until the goal trajectories have been reached. Based on the pose assignment, this function may call theOpenGripper
orCloseGripper
methods as is appropriate. -
Return to Unity. Select the Publisher GameObject and add the
TrajectoryPlanner
script as a component. -
Note that the TrajectoryPlanner component shows its member variables in the Inspector window, which need to be assigned.
Once again, drag and drop the
Target
andTargetPlacement
objects onto the Target and Target Placement Inspector fields, respectively. Assign theniryo_one
robot to the Niryo One field. Finally, assign the RosConnect object to theRos
field. -
Select the previously made Button object in Canvas/Button, and scroll to see the Button component. Under the
OnClick()
header, click the dropdown where it is currently assigned to the SourceDestinationPublisher.Publish(). Replace this call with TrajectoryPlanner >PublishJoints()
. -
The Unity side is now ready to communicate with ROS to motion plan!
The ROS Side
Note: This project was built using the ROS Melodic distro, and Python 2.
Note the file
src/niryo_moveit/scripts/mover.py
. This script holds the ROS-side logic for the MoverService. When the service is called, the functionplan_pick_and_place()
runs. This callsplan_trajectory
on the current joint configurations (sent from Unity) to a destination pose (dependent on the phase of the pick-and-place task).
def plan_trajectory(move_group, destination_pose, start_joint_angles):
current_joint_state = JointState()
current_joint_state.name = joint_names
current_joint_state.position = start_joint_angles
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = current_joint_state
move_group.set_start_state(moveit_robot_state)
move_group.set_pose_target(destination_pose)
plan = move_group.go(wait=True)
if not plan:
print("RAISE NO PLAN ERROR")
return move_group.plan()
This creates a set of planned trajectories, iterating through a pre-grasp, grasp, pick up, and place set of poses. Finally, this set of trajectories is sent back to Unity.
-
If you have not already built and sourced the ROS workspace since importing the new ROS packages, navigate to your ROS workplace, e.g.
Unity-Robotics-Hub/tutorials/pick_and_place/ROS/
, runcatkin_make && source devel/setup.bash
. Ensure there are no errors. -
If you have not already set the ROS parameter values in the
params.yaml
, navigate tosrc/niryo_moveit/config/params.yaml
and open the file for editing. Follow the steps in Part 2 to configure the values. -
The ROS side is now ready to interface with Unity!
ROS–Unity Communication
-
Open a new terminal window in the ROS workspace. Once again, source the workspace.
Then, run the following
roslaunch
in order to start roscore, set the ROS parameters, start the server endpoint, start the Mover Service node, and launch MoveIt.-
This launch file also loads all relevant files and starts ROS nodes required for trajectory planning for the Niryo One robot (
demo.launch
). The launch files for this project are available in the package'slaunch
directory, i.e.src/niryo_moveit/launch/
.Note: Descriptions of what these files are doing can be found here.
roslaunch niryo_moveit part_3.launch
This launch will print various messages to the console, including the set parameters and the nodes launched. The final two messages should confirm
You can start planning now!
andReady to plan
.Note: This may print out various error messages such as
Failed to find 3D sensor plugin
. These messages are safe to ignore as long as the final message to the console isYou can start planning now!
. -
-
Return to the Unity Editor and press Play. Press the UI Button to send the joint configurations to ROS, and watch the robot arm pick up and place the cube!
- The target object and placement positions can be moved around during runtime for different trajectory calculations.
Resources
Troubleshooting
Errors and Warnings
-
If the motion planning script throws a
RuntimeError: Unable to connect to move_group action server 'move_group' within allotted time (5s)
, ensure theroslaunch niryo_moveit part_3.launch
process launched correctly and has printedYou can start planning now!
. -
...failed because unknown error handler name 'rosmsg'
This is due to a bug in an outdated package version. Try runningsudo apt-get update && sudo apt-get upgrade
to upgrade.
Hangs, Timeouts, and Freezes
- If Unity fails to find a network connection, ensure that the ROS IP address is entered correctly as the Host Name in the RosConnect in Unity, and that the
src/niryo_moveit/config/params.yaml
values are set correctly.
Miscellaneous Issues
-
If the robot appears loose/wiggly or is not moving with no console errors, ensure that the Stiffness and Damping values on the Controller script of the
niryo_one
object are set to10000
and100
, respectively. -
If the robot moves to the incorrect location, or executes the poses in an expected order, verify that the shoulder_link (i.e.
niryo_one/world/base_link/shoulder_link
) X Drive Force Limit is5
. -
Before entering Play mode in the Unity Editor, ensure that all ROS processes are still running. The
server_endpoint
node may time out and will need to be re-run.