|
Module ee_cart_imped_action
|
|
1
2
3
4 import roslib; roslib.load_manifest('ee_cart_imped_action')
5 import rospy
6 import actionlib
7 import ee_cart_imped_msgs.msg
8
9 MAX_TRANS_STIFFNESS=1000.0
10 '''
11 Maximum allowed stiffness for position.
12 '''
13
14 MAX_ROT_STIFFNESS=100.0
15 '''
16 Maximum allowed stiffness for rotation.
17 '''
18
19
21 '''
22 A wrapper around the simple simple action client for the
23 EECartImpedAction.
24
25 See the actionlib documentation for more details on action clients.
26 '''
28 '''
29 Initialization of the client.
30 @type arm_name: string
31 @param arm_name: Which arm. Must either be 'right_arm' or 'left_arm'.
32 '''
33
34 self.arm_name = arm_name
35 '''
36 The name of the arm this client executes on.
37 '''
38
39 self.client =\
40 actionlib.SimpleActionClient\
41 ('/'+arm_name[0]+\
42 '_arm_cart_imped_controller/ee_cart_imped_action',\
43 ee_cart_imped_msgs.msg.EECartImpedAction)
44 '''
45 The simple action client used to communicate with the action server.
46 '''
47
48 rospy.loginfo("Waiting for ee_cart_imped action server for arm %s",
49 self.arm_name)
50 self.client.wait_for_server()
51 rospy.loginfo("Found ee_cart_imped action server")
52
53 self.goal = ee_cart_imped_msgs.msg.EECartImpedGoal()
54 '''
55 The current stored trajectory.
56 '''
57
58 self.resetGoal()
59
61 '''
62 Moves the arm to a pose stamped using the force/impedance controller.
63 @type pose_stamped: geometry_msgs.msg.PoseStamped
64 @param pose_stamped: The pose to move the end effector to.
65 @type time: double
66 @param time: The time after which this goal should be completed.
67 '''
68 goal = ee_cart_imped_msgs.msg.EECartImpedGoal()
69 new_point = len(goal.trajectory);
70 goal.trajectory.append(ee_cart_imped_msgs.msg.StiffPoint())
71 goal.trajectory[new_point].header.stamp =\
72 pose_stamped.header.stamp
73 goal.trajectory[new_point].header.frame_id =\
74 pose_stamped.header.frame_id
75 goal.trajectory[new_point].pose = pose_stamped.pose
76 goal.trajectory[new_point].wrench_or_stiffness.force.x =\
77 MAX_TRANS_STIFFNESS;
78 goal.trajectory[new_point].wrench_or_stiffness.force.y =\
79 MAX_TRANS_STIFFNESS;
80 goal.trajectory[new_point].wrench_or_stiffness.force.z =\
81 MAX_TRANS_STIFFNESS;
82 goal.trajectory[new_point].wrench_or_stiffness.torque.x =\
83 MAX_ROT_STIFFNESS;
84 goal.trajectory[new_point].wrench_or_stiffness.torque.y =\
85 MAX_ROT_STIFFNESS;
86 goal.trajectory[new_point].wrench_or_stiffness.torque.z =\
87 MAX_ROT_STIFFNESS;
88 goal.trajectory[new_point].isForceX = False
89 goal.trajectory[new_point].isForceY = False
90 goal.trajectory[new_point].isForceZ = False
91 goal.trajectory[new_point].isTorqueX = False
92 goal.trajectory[new_point].isTorqueY = False
93 goal.trajectory[new_point].isTorqueZ = False
94 goal.trajectory[new_point].time_from_start =\
95 rospy.Duration(time);
96 goal.header.stamp = rospy.Time.now()
97 rospy.logdebug('Sending pose goal %s', str(goal))
98 self.client.send_goal_and_wait(goal,
99 rospy.Duration(time + 2))
100
101
102 - def addTrajectoryPoint(self, x, y, z, ox, oy, oz, ow,
103 fx, fy, fz, tx, ty, tz, isfx, isfy, isfz,
104 istx, isty, istz, time, frame_id=''):
105 '''
106 Add a trajectory point to the current goal.
107 @type x: double
108 @param x: x position of the end effector
109 @type y: double
110 @param y: y position of the end effector
111 @type z: double
112 @param z: z position of the end effector
113 @type ox: double
114 @param ox: x component of the quaternion for the end effector rotation
115 @type oy: double
116 @param oy: y component of the quaternion for the end effector rotation
117 @type oz: double
118 @param oz: z component of the quaternion for the end effector rotation
119 @type ow: double
120 @param ow: w component of the quaternion for the end effector rotation
121 @type fx: double
122 @param fx: force or stiffness in the x direction
123 @type fy: double
124 @param fy: force or stiffness in the y direction
125 @type fz: double
126 @param fz: force or stiffness in the z direction
127 @type tx: double
128 @param tx: torque or stiffness around the x axis
129 @type ty: double
130 @param ty: torque or stiffness around the y axis
131 @type tz: double
132 @param tz: torque or stiffness around the z axis
133 @type isfx: boolean
134 @param isfx: True if this point should exert the given force in the x direction, false if it should achieve a position in the x direction with the given stiffness.
135 @type isfy: boolean
136 @param isfy: True if this point should exert the given force in the y direction, false if it should achieve a position in the y direction with the given stiffness.
137 @type isfz: boolean
138 @param isfz: True if this point should exert the given force in the z direction, false if it should achieve a position in the z direction with the given stiffness.
139 @type istx: boolean
140 @param istx: True if this point should exert the given torque around the x axis, false if it should achieve an orientation around the x axis with the given stiffness.
141 @type isty: boolean
142 @param isty: True if this point should exert the given torque around the y axis, false if it should achieve an orientation around the y axis with the given stiffness.
143 @type istz: boolean
144 @param istz: True if this point should exert the given torque around the z axis, false if it should achieve an orientation around the z axis with the given stiffness.
145 @type time: boolean
146 @param time: The time from start when this point should be achieved. Note that this is NOT the time from the last point, but the time from when the entire trajectory is begun.
147 @type frame_id: string
148 @param frame_id: The frame id of this point. If left as an empty string will be assumed that the point is in the frame of the root link of the chain for ee_cart_imped_control (likely torso_lift_link).
149 '''
150 new_point = len(self.goal.trajectory);
151 self.goal.trajectory.append(ee_cart_imped_msgs.msg.StiffPoint())
152 self.goal.trajectory[new_point].header.stamp = rospy.Time(0)
153 self.goal.trajectory[new_point].header.frame_id = frame_id
154 self.goal.trajectory[new_point].pose.position.x = x;
155 self.goal.trajectory[new_point].pose.position.y = y;
156 self.goal.trajectory[new_point].pose.position.z = z;
157 self.goal.trajectory[new_point].pose.orientation.x = ox;
158 self.goal.trajectory[new_point].pose.orientation.y = oy;
159 self.goal.trajectory[new_point].pose.orientation.z = oz;
160 self.goal.trajectory[new_point].pose.orientation.w = ow;
161 self.goal.trajectory[new_point].wrench_or_stiffness.force.x = fx;
162 self.goal.trajectory[new_point].wrench_or_stiffness.force.y = fy;
163 self.goal.trajectory[new_point].wrench_or_stiffness.force.z = fz;
164 self.goal.trajectory[new_point].wrench_or_stiffness.torque.x = tx;
165 self.goal.trajectory[new_point].wrench_or_stiffness.torque.y = ty;
166 self.goal.trajectory[new_point].wrench_or_stiffness.torque.z = tz;
167
168 self.goal.trajectory[new_point].isForceX = isfx
169 self.goal.trajectory[new_point].isForceY = isfy
170 self.goal.trajectory[new_point].isForceZ = isfz
171 self.goal.trajectory[new_point].isTorqueX = istx
172 self.goal.trajectory[new_point].isTorqueY = isty
173 self.goal.trajectory[new_point].isTorqueZ = istz
174 self.goal.trajectory[new_point].time_from_start = rospy.Duration(time);
175
177 '''
178 @return: The total time of the stored trajectory.
179 '''
180 if len(self.goal.trajectory) == 0:
181 return rospy.Duration(0)
182 return self.goal.trajectory[len(self.goal.trajectory)-1].time_from_start
183
185 '''
186 Sends the stored trajectory to the action server.
187 @type wait: boolean
188 @param wait: If true, this function will wait for the goal to complete before returning. If false, this function will return immediately upon sending the goal.
189 @return: The state of the simple action client.
190 '''
191 self.goal.header.stamp = rospy.Time.now()
192 rospy.logdebug('Sending goal %s to force controller on %s',
193 str(self.goal), self.arm_name)
194 self.client.send_goal(self.goal)
195 if wait:
196 self.client.wait_for_result()
197 return self.client.get_state()
198
200 '''
201 Resets the stored trajectory to be an empty trajectory.
202 '''
203 self.goal = ee_cart_imped_msgs.msg.EECartImpedGoal()
204 self.goal.header.frame_id = 'torso_lift_link'
205
207 '''
208 Cancels the current goal. The arm will hold its current position.
209 '''
210 self.client.cancel_goal()
211
213 '''
214 Cancels all goals on the action server. The arm will hold its current position.
215 '''
216 self.client.cancel_all_goals()
217