Skip to content

Commit b454a76

Browse files
URJalaURJala
committed
Update controller to match updated service definition
Also add example of using force mode controller
1 parent 62f02ea commit b454a76

File tree

5 files changed

+322
-16
lines changed

5 files changed

+322
-16
lines changed

‎ur_controllers/src/force_mode_controller.cpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -217,12 +217,12 @@ bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request
217217

218218
/* The limits specifies the maximum allowed speed along/around compliant axes. For non-compliant axes this value is
219219
* the maximum allowed deviation between actual tcp position and the one that has been programmed. */
220-
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(req->limits.twist.linear.x);
221-
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(req->limits.twist.linear.y);
222-
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(req->limits.twist.linear.z);
223-
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(req->limits.twist.angular.x);
224-
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(req->limits.twist.angular.y);
225-
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(req->limits.twist.angular.z);
220+
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(req->limits[0]);
221+
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(req->limits[1]);
222+
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(req->limits[2]);
223+
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(req->limits[3]);
224+
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(req->limits[4]);
225+
command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(req->limits[5]);
226226

227227
/* The type decides how the robot interprets the force frame (the one defined in task_frame). See ur_script manual for
228228
* explanation, under force_mode. */

‎ur_robot_driver/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -149,6 +149,8 @@ install(PROGRAMS
149149
scripts/tool_communication.py
150150
scripts/wait_for_robot_description
151151
scripts/start_ursim.sh
152+
examples/examples.py
153+
examples/force_mode.py
152154
DESTINATION lib/${PROJECT_NAME}
153155
)
154156

‎ur_robot_driver/examples/examples.py‎

Lines changed: 175 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,175 @@
1+
#!/usr/bin/env python3
2+
# Copyright 2024, Universal Robots A/S
3+
#
4+
# Redistribution and use in source and binary forms, with or without
5+
# modification, are permitted provided that the following conditions are met:
6+
#
7+
# * Redistributions of source code must retain the above copyright
8+
# notice, this list of conditions and the following disclaimer.
9+
#
10+
# * Redistributions in binary form must reproduce the above copyright
11+
# notice, this list of conditions and the following disclaimer in the
12+
# documentation and/or other materials provided with the distribution.
13+
#
14+
# * Neither the name of the {copyright_holder} nor the names of its
15+
# contributors may be used to endorse or promote products derived from
16+
# this software without specific prior written permission.
17+
#
18+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21+
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
22+
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23+
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24+
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25+
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26+
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27+
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28+
# POSSIBILITY OF SUCH DAMAGE.
29+
30+
import rclpy
31+
from builtin_interfaces.msg import Duration
32+
from control_msgs.action import FollowJointTrajectory
33+
34+
from rclpy.action import ActionClient
35+
from rclpy.node import Node
36+
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
37+
from ur_msgs.srv import SetIO
38+
from controller_manager_msgs.srv import SwitchController
39+
from std_srvs.srv import Trigger
40+
41+
TIMEOUT_WAIT_SERVICE = 10
42+
TIMEOUT_WAIT_SERVICE_INITIAL = 60
43+
TIMEOUT_WAIT_ACTION = 10
44+
45+
ROBOT_JOINTS = [
46+
"shoulder_pan_joint",
47+
"shoulder_lift_joint",
48+
"elbow_joint",
49+
"wrist_1_joint",
50+
"wrist_2_joint",
51+
"wrist_3_joint",
52+
]
53+
54+
55+
# Helper functions
56+
def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE):
57+
client = node.create_client(srv_type, srv_name)
58+
if client.wait_for_service(timeout) is False:
59+
raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}")
60+
61+
node.get_logger().info(f"Successfully connected to service '{srv_name}'")
62+
return client
63+
64+
65+
def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION):
66+
client = ActionClient(node, action_type, action_name)
67+
if client.wait_for_server(timeout) is False:
68+
raise Exception(
69+
f"Could not reach action server '{action_name}' within timeout of {timeout}"
70+
)
71+
72+
node.get_logger().info(f"Successfully connected to action '{action_name}'")
73+
return client
74+
75+
76+
class Robot:
77+
def __init__(self, node):
78+
self.node = node
79+
self.service_interfaces = {
80+
"/io_and_status_controller/set_io": SetIO,
81+
"/dashboard_client/play": Trigger,
82+
"/controller_manager/switch_controller": SwitchController,
83+
}
84+
self.init_robot()
85+
86+
def init_robot(self):
87+
self.service_clients = {
88+
srv_name: waitForService(self.node, srv_name, srv_type)
89+
for (srv_name, srv_type) in self.service_interfaces.items()
90+
}
91+
92+
self.jtc_action_client = waitForAction(
93+
self.node,
94+
"/scaled_joint_trajectory_controller/follow_joint_trajectory",
95+
FollowJointTrajectory,
96+
)
97+
98+
def set_io(self, pin, value):
99+
"""Test to set an IO."""
100+
set_io_req = SetIO.Request()
101+
set_io_req.fun = 1
102+
set_io_req.pin = pin
103+
set_io_req.state = value
104+
105+
self.call_service("/io_and_status_controller/set_io", set_io_req)
106+
107+
def send_trajectory(self, waypts, time_vec):
108+
"""Send robot trajectory."""
109+
if len(waypts) != len(time_vec):
110+
raise Exception("waypoints vector and time vec should be same length")
111+
112+
# Construct test trajectory
113+
joint_trajectory = JointTrajectory()
114+
joint_trajectory.joint_names = ROBOT_JOINTS
115+
for i in range(len(waypts)):
116+
point = JointTrajectoryPoint()
117+
point.positions = waypts[i]
118+
point.time_from_start = time_vec[i]
119+
joint_trajectory.points.append(point)
120+
121+
# Sending trajectory goal
122+
goal_response = self.call_action(
123+
self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory)
124+
)
125+
if not goal_response.accepted:
126+
raise Exception("trajectory was not accepted")
127+
128+
# Verify execution
129+
result = self.get_result(self.jtc_action_client, goal_response)
130+
return result.error_code == FollowJointTrajectory.Result.SUCCESSFUL
131+
132+
def call_service(self, srv_name, request):
133+
future = self.service_clients[srv_name].call_async(request)
134+
rclpy.spin_until_future_complete(self.node, future)
135+
if future.result() is not None:
136+
return future.result()
137+
else:
138+
raise Exception(f"Exception while calling service: {future.exception()}")
139+
140+
def call_action(self, ac_client, g):
141+
future = ac_client.send_goal_async(g)
142+
rclpy.spin_until_future_complete(self.node, future)
143+
144+
if future.result() is not None:
145+
return future.result()
146+
else:
147+
raise Exception(f"Exception while calling action: {future.exception()}")
148+
149+
def get_result(self, ac_client, goal_response):
150+
future_res = ac_client._get_result_async(goal_response)
151+
rclpy.spin_until_future_complete(self.node, future_res)
152+
if future_res.result() is not None:
153+
return future_res.result().result
154+
else:
155+
raise Exception(f"Exception while calling action: {future_res.exception()}")
156+
157+
158+
if __name__ == "__main__":
159+
rclpy.init()
160+
node = Node("robot_driver_test")
161+
robot = Robot(node)
162+
163+
# The following list are arbitrary joint positions, change according to your own needs
164+
waypts = [
165+
[-1.6006, -1.7272, -2.2030, -0.8079, 1.5951, -0.0311],
166+
[-1.2, -1.4, -1.9, -1.2, 1.5951, -0.0311],
167+
[-1.6006, -1.7272, -2.2030, -0.8079, 1.5951, -0.0311],
168+
]
169+
time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)]
170+
171+
# Execute trajectory on robot, make sure that the robot is booted and the control script is running
172+
robot.send_trajectory(waypts, time_vec)
173+
174+
# Set digital output 1 to true
175+
robot.set_io(1, 1.0)

‎ur_robot_driver/examples/force_mode.py‎

Lines changed: 136 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,136 @@
1+
#!/usr/bin/env python3
2+
# Copyright 2024, Universal Robots A/S
3+
#
4+
# Redistribution and use in source and binary forms, with or without
5+
# modification, are permitted provided that the following conditions are met:
6+
#
7+
# * Redistributions of source code must retain the above copyright
8+
# notice, this list of conditions and the following disclaimer.
9+
#
10+
# * Redistributions in binary form must reproduce the above copyright
11+
# notice, this list of conditions and the following disclaimer in the
12+
# documentation and/or other materials provided with the distribution.
13+
#
14+
# * Neither the name of the {copyright_holder} nor the names of its
15+
# contributors may be used to endorse or promote products derived from
16+
# this software without specific prior written permission.
17+
#
18+
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19+
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20+
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21+
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
22+
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23+
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24+
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25+
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26+
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27+
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28+
# POSSIBILITY OF SUCH DAMAGE.
29+
30+
import time
31+
32+
import rclpy
33+
from rclpy.node import Node
34+
from controller_manager_msgs.srv import SwitchController
35+
from builtin_interfaces.msg import Duration
36+
from std_msgs.msg import Header
37+
from std_srvs.srv import Trigger
38+
39+
from geometry_msgs.msg import (
40+
Point,
41+
Quaternion,
42+
Pose,
43+
PoseStamped,
44+
Wrench,
45+
WrenchStamped,
46+
Vector3,
47+
)
48+
49+
from ur_msgs.srv import SetForceMode
50+
51+
from examples import Robot
52+
53+
if __name__ == "__main__":
54+
rclpy.init()
55+
node = Node("robot_driver_test")
56+
robot = Robot(node)
57+
58+
# Activate force mode controller
59+
robot.call_service(
60+
"/controller_manager/switch_controller",
61+
SwitchController.Request(
62+
activate_controllers=["force_mode_controller", "scaled_joint_trajectory_controller"],
63+
strictness=SwitchController.Request.BEST_EFFORT,
64+
),
65+
)
66+
67+
# Add force mode service to service interfaces and re-init robot
68+
robot.service_interfaces.update({"/force_mode_controller/start_force_mode": SetForceMode})
69+
robot.init_robot()
70+
time.sleep(2)
71+
# Press play on the robot
72+
robot.call_service("/dashboard_client/play", Trigger.Request())
73+
74+
# Move robot in to position
75+
robot.send_trajectory(
76+
waypts=[[-1.5707, -1.5707, -1.5707, -1.5707, 1.5707, 0]],
77+
time_vec=[Duration(sec=6, nanosec=0)],
78+
)
79+
80+
# Finished moving
81+
82+
# Create task frame for force mode
83+
point = Point(x=0.0, y=0.0, z=0.0)
84+
orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=0.0)
85+
task_frame_pose = Pose()
86+
task_frame_pose.position = point
87+
task_frame_pose.orientation = orientation
88+
header = Header(seq=1, frame_id="world")
89+
header.stamp.sec = int(time.time()) + 1
90+
header.stamp.nanosec = 0
91+
frame_stamp = PoseStamped()
92+
frame_stamp.header = header
93+
frame_stamp.pose = task_frame_pose
94+
95+
# Create compliance vector (which axes should be force controlled)
96+
compliance = [False, False, True, False, False, False]
97+
98+
# Create Wrench message for force mode
99+
wrench_vec = Wrench(force=Vector3(x=0.0, y=0.0, z=-10.0), torque=Vector3(x=0.0, y=0.0, z=0.0))
100+
wrench_stamp = WrenchStamped(header=header, wrench=wrench_vec)
101+
# Specify interpretation of task frame (no transform)
102+
type_spec = SetForceMode.Request.NO_TRANSFORM
103+
104+
# Specify max speeds and deviations of force mode
105+
limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
106+
107+
# specify damping and gain scaling
108+
damping_factor = 0.025
109+
gain_scale = 0.5
110+
111+
req = SetForceMode.Request()
112+
req.task_frame = frame_stamp
113+
req.selection_vector_x = compliance[0]
114+
req.selection_vector_y = compliance[1]
115+
req.selection_vector_z = compliance[2]
116+
req.selection_vector_rx = compliance[3]
117+
req.selection_vector_ry = compliance[4]
118+
req.selection_vector_rz = compliance[5]
119+
req.wrench = wrench_stamp
120+
req.type = type_spec
121+
req.limits = limits
122+
req.damping_factor = damping_factor
123+
req.gain_scaling = gain_scale
124+
125+
# Send request to controller
126+
robot.call_service("/force_mode_controller/start_force_mode", req)
127+
128+
time.sleep(15)
129+
# Deactivate force mode controller
130+
robot.call_service(
131+
"/controller_manager/switch_controller",
132+
SwitchController.Request(
133+
deactivate_controllers=["force_mode_controller"],
134+
strictness=SwitchController.Request.BEST_EFFORT,
135+
),
136+
)

‎ur_robot_driver/test/robot_driver.py‎

Lines changed: 3 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -48,9 +48,7 @@
4848
Quaternion,
4949
Point,
5050
WrenchStamped,
51-
TwistStamped,
5251
Wrench,
53-
Twist,
5452
Vector3,
5553
)
5654
from ur_msgs.msg import IOStates
@@ -186,15 +184,10 @@ def test_force_mode_controller(self, tf_prefix):
186184
type_spec = 2
187185

188186
# Specify max speeds and deviations of force mode
189-
limits = Twist()
190-
limits.linear = Vector3(x=1.1, y=1.1, z=1.1)
191-
limits.angular = limits.linear
192-
limits_stamp = TwistStamped()
193-
limits_stamp.header = header
194-
limits_stamp.twist = limits
187+
limits = [1.0, 1.0, 1.0, 0.5, 1.0, 1.0, 1.0]
195188

196189
# specify damping and gain scaling
197-
damping_factor = 0.8
190+
damping_factor = 0.1
198191
gain_scale = 0.8
199192

200193
# Send request to controller
@@ -208,7 +201,7 @@ def test_force_mode_controller(self, tf_prefix):
208201
selection_vector_rz=compliance[5],
209202
wrench=wrench_stamp,
210203
type=type_spec,
211-
limits=limits_stamp,
204+
limits=limits,
212205
damping_factor=damping_factor,
213206
gain_scaling=gain_scale,
214207
)

0 commit comments

Comments
 (0)