Rapid code corresponding to abb_libegm

[dyumanaditya]

Hi All, My objective is to create a ROS node that subscribes to a stream of poses being sent on a certain topic. This node should then send these poses to RobotStudio for the robot to follow. I've successfully managed to create a node that can send these poses to RobotStudio, here is my callback function when a pose is published: (I'm omitting the rest of the code because the communication setup is identical to the abb_libegm_samples)

Click to expand

```c++ void Teleoperation::poseEstimationCallback(const geometry_msgs::PoseStamped& msg) { ROS_INFO("callback"); abb::egm::wrapper::Input input; abb::egm::wrapper::Output output; abb::egm::wrapper::CartesianPose pose; // Parse msg double pos_x = msg.pose.position.x; double pos_y = msg.pose.position.y; double pos_z = msg.pose.position.z; double orn_x = msg.pose.orientation.x; double orn_y = msg.pose.orientation.y; double orn_z = msg.pose.orientation.z; double orn_w = msg.pose.orientation.w; // Wait for a new EGM message from the EGM client (with a timeout of 500 ms). if (egmInterface->waitForMessage(500)) { // Read the message received from the EGM client egmInterface->read(&input); sequenceNumber = input.header().sequence_number(); if (sequenceNumber == 0) { // Reset all references, if it is the first message. output.Clear(); pose.CopyFrom(input.feedback().robot().cartesian().pose()); output.mutable_robot()->mutable_cartesian()->mutable_pose()->CopyFrom(pose); } else { output.mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_position()->set_x(pos_x); output.mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_position()->set_y(pos_y); output.mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_position()->set_z(pos_z); output.mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_quaternion()->set_u0(orn_x); output.mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_quaternion()->set_u1(orn_y); output.mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_quaternion()->set_u2(orn_z); output.mutable_robot()->mutable_cartesian()->mutable_pose()->mutable_quaternion()->set_u3(orn_w); } // Write references back to the EGM client. egmInterface->write(output); } } ```

With this, I have tested sending a single pose to RobotStudio and the robot goes to that pose as expected. However after a few seconds, the robot moves back to it's home position. I need the robot to move only to the poses that I send to it. I'm not too sure my RAPID code that connects the ROS node and RobotStudio is setup correctly. Here is the code on TRob1Main:

Click to expand

```RAPID MODULE TRob1Main !====================================================================================================== ! Copyright (c) 2018, ABB Schweiz AG ! All rights reserved. ! ! Redistribution and use in source and binary forms, with ! or without modification, are permitted provided that ! the following conditions are met: ! ! * Redistributions of source code must retain the ! above copyright notice, this list of conditions ! and the following disclaimer. ! * Redistributions in binary form must reproduce the ! above copyright notice, this list of conditions ! and the following disclaimer in the documentation ! and/or other materials provided with the ! distribution. ! * Neither the name of ABB nor the names of its ! contributors may be used to endorse or promote ! products derived from this software without ! specific prior wrSitten permission. ! ! THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" ! AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE ! IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ! ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE ! LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL ! DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR ! SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER ! CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, ! OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF ! THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. !====================================================================================================== !*********************************************************** ! Program data !*********************************************************** ! Home position. LOCAL CONST jointtarget home := [[0, 0, 0, 0, 30, 0], [9E9, 9E9, 9E9, 9E9, 9E9, 9E9]]; ! Identifier for the EGM correction. LOCAL VAR egmident egm_id; ! Limits for convergance. LOCAL VAR egm_minmax egm_condition := [-0.1, 0.1]; ! EGM pose frames. LOCAL CONST pose egm_correction_frame := [[0, 0, 0], [1, 0, 0, 0]]; LOCAL CONST pose egm_sensor_frame := [[0, 0, 0], [1, 0, 0, 0]]; ! The work object (here set to coincidence with ROB_1's base frame). LOCAL PERS wobjdata egm_wobj := [FALSE, TRUE, "", [[0, 0, 0],[1, 0, 0, 0]], [[0, 0, 0],[1, 0, 0, 0]]]; !*********************************************************** ! ! Procedure main ! ! This RAPID code exemplify how to run EGM pose motions. ! ! Note: Update the UCDevice "ROB_1" with correct ! values for the remote address and port ! (i.e. to the EGM server). ! ! Update via RobotStudio: ! Controller tab -> Configuration -> ! Communication -> Transmission Protocol ! !*********************************************************** PROC main() MoveAbsJ home, v200, fine, tool0; ! Register an EGM id. EGMGetId egm_id; ! Setup the EGM communication. EGMSetupUC ROB_1, egm_id, "default", "ROB_1", \Pose; ! Prepare for an EGM communication session. EGMActPose egm_id, \WObj:=egm_wobj, egm_correction_frame, EGM_FRAME_BASE, egm_sensor_frame, EGM_FRAME_BASE \X:=egm_condition \Y:=egm_condition \Z:=egm_condition \Rx:=egm_condition \Ry:=egm_condition \Rz:=egm_condition \MaxSpeedDeviation:=20.0; WHILE TRUE DO ! Start the EGM communication session. EGMRunPose egm_id, EGM_STOP_RAMP_DOWN, \X \Y \Z \Rx \Ry \Rz \CondTime:=5 \RampOutTime:=5; WaitTime 5; ENDWHILE ERROR IF ERRNO = ERR_UDPUC_COMM THEN TPWrite "Communication timedout"; TRYNEXT; ENDIF ENDPROC ENDMODULE ```

How can I correct this RAPID code so that the robot moves only to the streamed position and doesn't keep going to the "home pose" ? If this code is not the correct way to receive a stream of poses, what is a suitable way? Any help will be much appreciated, thanks in advance.

[jontje]

You would need to change EGM_STOP_RAMP_DOWN to EGM_STOP_HOLD in the EGMRunPose routine.

The meaning of EGM_STOP_RAMP_DOWN is that the robot will return to where EGM motions started, after the EGM communication session ends.

[dyumanaditya]

Thank you @jontje, that fixed it. When I stream poses on a ROS topic, the robot in RobotStudio finishes every pose before starting the next. There seems to be a sort of blocking operation. How can I make the robot move to the published pose in real-time as soon as it's been published -- i.e. not visit every pose that is published, but only move towards the most recent one? Is it possible to do this?

[dyumanaditya]

UPDATE: I apologize for the previous comment, it turns out there was some latency regarding when I published the pose and when the robot started moving. Is there some way to reduce this latency? When I publish the pose it sometimes takes as long as 5-7 seconds for the robot to start moving in robot studio.

Sometimes the robot doesn't register the published pose; I have to re-publish for it to register and move to that pose.

[jontje]

When I publish the pose it sometimes takes as long as 5-7 seconds for the robot to start moving in robot studio.

That is probably because the RAPID example code you are using includes a WaitTime 5; instruction.

That RAPID example code shouldn't be used for any real application, its intention is just to show the necessary RAPID instructions to setup and start EGM on the robot controller side.

If possible, then I suggest that you use the StateMachine Add-In instead, which provides RAPID code for starting EGM pose mode via an IO-signal (which can be triggered via RWS communication).

[dyumanaditya]

That is probably because the RAPID example code you are using includes a WaitTime 5; instruction.

Even without the WaitTime 5; instruction the delay/latency stays the same; I have to publish twice sometimes for it to be registered.

If possible, then I suggest that you use the StateMachine Add-In instead, which provides RAPID code for starting EGM pose mode via an IO-signal (which can be triggered via RWS communication).

Thanks for this, I will try it out.

[gavanderhoorn]

I'm going to close this due to inactivity.