Controller.py 9.12 KB
Newer Older
1
#!/usr/bin/env python
2
DEBUG = True
3
4
VERBOSE = True

Cadmus's avatar
Cadmus committed
5
## Python libraries
Lab Computer's avatar
Lab Computer committed
6
from copy import deepcopy
7
import time
8
import sys
9

Cadmus's avatar
Cadmus committed
10
11
## 3rd party libraries
# Scipy (1.2.3)
Lab Computer's avatar
Lab Computer committed
12
from scipy.spatial.transform import Rotation
13

Cadmus's avatar
Cadmus committed
14
# Numpy (1.16.6)
Lab Computer's avatar
Lab Computer committed
15
16
import numpy as np

Cadmus's avatar
Cadmus committed
17
# ROS Libraries
Jayant Khatkar's avatar
Jayant Khatkar committed
18
import rospy
19
from std_msgs.msg import Bool
Jayant Khatkar's avatar
Jayant Khatkar committed
20
21
22
23
from trajectory_msgs.msg import (
    JointTrajectoryPoint,
    JointTrajectory
)
24
from actionlib_msgs.msg import GoalStatusArray
25
26
27

# MOVEit
import moveit_commander
28
from moveit_msgs.msg._RobotTrajectory import RobotTrajectory
29

30
31
32
## Our libraries
from extrudex.msg._ExtruderControl import ExtruderControl
from extrudex.srv._Hotend import Hotend
33

Cadmus's avatar
Cadmus committed
34
35
36
37
class Controller(object):
    """
    Controller for multiple robot arms to execute lists of configurations with timestamps.
    The timing of the arms include extrusion of filament.
Lab Computer's avatar
Lab Computer committed
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

    Frame names:
    - base_link
    - shoulder_link
    - upper_arm_link
    - forearm_link
    - wrist_1_link
    - wrist_2_link
    - wrist_3_link

    Joint names:
    - shoulder_pan_joint
    - shoulder_lift_joint
    - elbow_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint
Cadmus's avatar
Cadmus committed
55
    """
56
    def __init__(self,
Cadmus's avatar
Cadmus committed
57
        close_deg=1.0,
58
59
60
61
62
        timeScale=1.0,
        blocking=True,
        extruderTemp_C=190.0,
        hotend_srv='hotend',
        retractionDist_mm=12.0,
63
        retractionSpeed_mmpmin=4800.0,
64
        disable_extruder=False):
Jayant Khatkar's avatar
Jayant Khatkar committed
65
        """
Cadmus's avatar
Cadmus committed
66
67
68
        Creates a controller to multiple robot arms. (TODO)

        close_deg: Euclidean distance of configurations within which two configurations are considered "close"
69
        timeScale: Scaling factor of time
Jayant Khatkar's avatar
Jayant Khatkar committed
70
        """
71
72
73
74
75
76
77
78
79
        ## Initialise ROS things
        self.node = rospy.init_node('py_controller',
            anonymous = False)
        self.extrusion_pub = rospy.Publisher('extrusion_cmd',ExtruderControl,
            queue_size=2)
        self.extruderReady_sub = rospy.Subscriber('ready',Bool,self._extruderReady_cb,
            queue_size=1)
        self.scaledPosTraj_pub = rospy.Publisher('scaled_pos_traj_controller/command',JointTrajectory,
            queue_size=1)
80
81
        self.motion_feedback_sub = rospy.Subscriber('scaled_pos_traj_controller/follow_joint_trajectory/status',GoalStatusArray,
            self._update_motion_status)
82
83

        time.sleep(0.1)
84
85
        if not disable_extruder:
            print("Waiting for Hotend service")
86
            rospy.wait_for_service(hotend_srv)
87
88
89
90
            try:
                self.extruderSetTemp_srv = rospy.ServiceProxy(hotend_srv,Hotend)
            except rospy.ServiceException as e:
                print('Service call failed: {}'.format(e))
Jayant Khatkar's avatar
Jayant Khatkar committed
91

92
93
94
95
96
        # initialise MoveIt commander
        moveit_commander.roscpp_initialize(sys.argv)

        self.robot = moveit_commander.RobotCommander()
        self.scene = moveit_commander.PlanningSceneInterface()
97
        self.group = moveit_commander.MoveGroupCommander('manipulator')
98

99
100
        self.group.set_max_velocity_scaling_factor(timeScale)
        self.group.set_max_acceleration_scaling_factor(timeScale**2)
101

102
103
104
105
106
        self.close_deg = close_deg
        self.timeScale = timeScale
        self.retractionDist_mm = retractionDist_mm
        self.retractionTime_s = retractionDist_mm/retractionSpeed_mmpmin * 60.0
        self.extruderReady = False
107
        self.traj_complete = False
108
109

        # Start extruder
110
        if not disable_extruder:
111
112
113
            self.set_extruder_temp(extruderTemp_C)
            if blocking:
                self.wait_extruder_ready()
114

115
116

    ## Arm methods
Cadmus's avatar
Cadmus committed
117
    def get_joint_names(self):
118
        """
Cadmus's avatar
Cadmus committed
119
        Returns the list of joint names in the order the controller uses
120
        """
Cadmus's avatar
Cadmus committed
121
        return self.group.get_active_joints()
122
123


Cadmus's avatar
Cadmus committed
124
    def empty_traj(self):
125
        """
Cadmus's avatar
Cadmus committed
126
        Creates an empty JointTrajectory with joint names of the arm prefilled.
127

Cadmus's avatar
Cadmus committed
128
129
130
131
132
        Returns JointTrajectory object.
        """
        t = JointTrajectory()
        t.joint_names = self.get_joint_names()
        return t
133
134


135
    def exec_traj(self,traj):
Jayant Khatkar's avatar
Jayant Khatkar committed
136
        """
Cadmus's avatar
Cadmus committed
137
        Executes a JointTrajectory object
138

Cadmus's avatar
Cadmus committed
139
140
141
142
143
        NOTE: If the arm is not at the first JointTrajectoryPoint, then
              the trajectory is not executed.

        traj: JointTrajectory object to execute
        Returns True if the JointTrajectory is executed; False otherwise.
Jayant Khatkar's avatar
Jayant Khatkar committed
144
        """
145
        rospy.loginfo('Sending trajectory to UR5e')
146

147
        # Scale trajectory's temporal aspect
Lab Computer's avatar
Lab Computer committed
148
149
        scaledTraj = deepcopy(traj)
        for (iPoint,point) in enumerate(traj.points):
150
            # Scale timestamps
Lab Computer's avatar
Lab Computer committed
151
            t = point.time_from_start.secs + 1e-9*point.time_from_start.nsecs
152
            scaledT = t/self.timeScale
Lab Computer's avatar
Lab Computer committed
153
            secs = int(scaledT)
154
            nsecs = int((scaledT-secs)*1e9)
Lab Computer's avatar
Lab Computer committed
155
156
157
            scaledTraj.points[iPoint].time_from_start.secs = secs
            scaledTraj.points[iPoint].time_from_start.nsecs = nsecs

158
159
160
161
162
163
164
            # Scale velocity
            scaledTraj.points[iPoint].velocities = [v*self.timeScale for v in scaledTraj.points[iPoint].velocities]

            # Scale acceleration
            timeScale2 = self.timeScale**2
            scaledTraj.points[iPoint].accelerations = [a*timeScale2 for a in scaledTraj.points[iPoint].accelerations]

165
        plan = RobotTrajectory()
Lab Computer's avatar
Lab Computer committed
166
        plan.joint_trajectory = scaledTraj
167
168
169
        self.group.execute(plan, wait=False)

        s = time.time()
170
171
172
173
174
175
        t = traj.points[-1].time_from_start.to_sec()
        while not self.traj_complete:
            if time.time() - s > t + 3:
                Exception("traj_complete failed")
                break
        self.traj_complete = False
Jayant Khatkar's avatar
Jayant Khatkar committed
176

Cadmus's avatar
Cadmus committed
177
        return True
178
179


180
    def move_to(self,conf_next,near_deg = 1.0):
Cadmus's avatar
Cadmus committed
181
182
183
184
        """
        Moves the arm to the specified configuration by linearly
        interpolating the joint angles.

Lab Computer's avatar
Lab Computer committed
185
        conf_next: 6x numpy array of joint angles in radians
Cadmus's avatar
Cadmus committed
186
187
        near_deg: joint distance limit in degrees [default 1.0];
                  if None, no check is done.
188
        """
189
        if DEBUG:
Cadmus's avatar
Cadmus committed
190
            print('Controller::move_to()')
191

Cadmus's avatar
Cadmus committed
192
        if near_deg is not None:
193
            # List of joint angles
Lab Computer's avatar
Lab Computer committed
194
            conf_curr = np.array(self.get_joints())
195

196
            # Make sure the new joint angles are not too different
Lab Computer's avatar
Lab Computer committed
197
            dConf = conf_next-conf_curr
Cadmus's avatar
Cadmus committed
198
199
200
            dist_rad = np.linalg.norm(dConf)
            dist_deg = np.rad2deg(dist_rad)
            if dist_deg > near_deg:
Lab Computer's avatar
Lab Computer committed
201
                rospy.logwarn('Attempted to move a joint distance of {} > {} deg'.format(dist_conf_deg,jointLim_deg))
202
                return False
Jayant Khatkar's avatar
Jayant Khatkar committed
203

Lab Computer's avatar
Lab Computer committed
204
        self.group.go(conf_next)
205
        return True
Jayant Khatkar's avatar
Jayant Khatkar committed
206

207

208
    def is_close(self,trajPoint):
209
        """
Lab Computer's avatar
Lab Computer committed
210
        Check if the arm is currently close to the configuration specified.
Cadmus's avatar
Cadmus committed
211
        Ignores timestamp.
212

Cadmus's avatar
Cadmus committed
213
        trajPoint: JointTrajectoryPoint configuration
214
        """
Lab Computer's avatar
Lab Computer committed
215
216
217
218
219
220
221
222
223
        # TODO: Check if the joint order is the same
        conf_ref = np.array(trajPoint.positions)
        conf_curr = np.array(self.get_joints())

        dConf = conf_ref-conf_curr
        dist_rad = np.linalg.norm(dConf)
        dist_deg = np.rad2deg(dist_rad)

        return dist_deg <= self.close_deg
Lab Computer's avatar
Lab Computer committed
224
225


Cadmus's avatar
Cadmus committed
226
    def get_joints(self):
Jayant Khatkar's avatar
Jayant Khatkar committed
227
        """
Cadmus's avatar
Cadmus committed
228
        Return configuration of the arm in a list
Jayant Khatkar's avatar
Jayant Khatkar committed
229
        """
Cadmus's avatar
Cadmus committed
230
        return self.group.get_current_joint_values()
Jayant Khatkar's avatar
Jayant Khatkar committed
231
232


233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
    ## Extruder methods
    def extrude(self,ext_mm,t_s):
        """
        Send raw message to move extrusion.

        ext_mm: float
        t_s: float
        """
        if DEBUG:
            print('Controller::extrude({},{})'.format(ext_mm,t_s))
        msg = ExtruderControl()
        msg.delta_e = ext_mm
        msg.delta_t = t_s
        self.extrusion_pub.publish(msg)


    def postprint_retract(self):
        self.extrude(-self.retractionDist_mm,self.retractionTime_s)


    def preprint_load(self):
        self.extrude(self.retractionDist_mm,self.retractionTime_s)


    def printFilament(self,ext_mm,t_s):
        """
        Does a sequence of print with retractions.

        ext_mm: Tx
        t_s: Tx
        """
        nStride = ext_mm.shape[0]
        self.preprint_load()

        for iStride in range(nStride):
            self.extrude(self,ext_mm,t_s)
            time.sleep(t_s)

        self.postprint_retract()


    def set_extruder_temp(self,temp_C):
        if VERBOSE:
            rospy.loginfo('Setting extruder temperature to {} C'.format(temp_C))
        self.extruderTemp_C = temp_C
        self.extruderSetTemp_srv(self.extruderTemp_C)


    def wait_extruder_ready(self):
        if VERBOSE:
            rospy.loginfo('Waiting for extruder to heat up...')
        while not self.extruderReady:
            time.sleep(0.1)
        if VERBOSE:
            rospy.loginfo('Done!')


    def _extruderReady_cb(self,ready_boolMsg):
        self.extruderReady = ready_boolMsg.data

293
294
295
296
    def _update_motion_status(self, msg):
        if len(msg.status_list)>0:
            self.traj_complete = (msg.status_list[-1].status==3 or
                msg.status_list[-1].status==2)
297
298
            if self.traj_complete:
                print("Trajectory Execution Completed")
299

300

301
def getR_ET():
302
    return Rotation.from_euler('XYZ',[-45,-90,0],
303
304
305
        degrees=True)


306
if __name__=='__main__':
Cadmus's avatar
Cadmus committed
307
    con = Controller()
308
309

# TODO: Add confirmation checks
Cadmus's avatar
Cadmus committed
310
311
312
# display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
#     moveit_msgs.msg.DisplayTrajectory,
#     queue_size=20)