def moveToDoor(): print "Ruch do dzwiczek..." Goal = PyKDL.Vector( 1 / 3 * doorL_x + 2 / 3 * doorR_x + 0.5 * math.cos(alfa), 1 / 3 * doorL_y + 2 / 3 * doorR_y + 0.5 * math.sin(alfa), doorR_z + 0.1) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(alfa), Goal) if not velma.moveCartImpRight( [T_B_Door], [10.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100, 100, 0.7), PyKDL.Vector(0.7, 0.7, 0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(0.2, 0.2, 1), PyKDL.Vector(1, 1, 1))): exitError(8) if velma.waitForEffectorRight() != 0: print "Velma: Dotykam drzwi :p" rospy.sleep(0.5) resetPoseTol() rospy.sleep(0.5)
def moveInCartImpMode(velma, T_B_dest): if not velma.moveCartImpRight([T_B_dest], [5.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5,5,5), PyKDL.Vector(5,5,5)), start_time=0.5): exitError(8) rospy.sleep(0.5) if velma.waitForEffectorRight() != 0: exitError(9) rospy.sleep(0.5)
def moveRight(x, y, z, theta): modeCart() print "Moving right wrist to position:", x, y, z, "\n" T_B_Trd = PyKDL.Frame(PyKDL.Rotation.RPY(0.0, 0.0, theta), PyKDL.Vector(x, y, z)) if not velma.moveCartImpRight([T_B_Trd], [3.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.5): exitError(8) if velma.waitForEffectorRight() != 0: exitError(9) rospy.sleep(0.5) print "calculating difference between desiread and reached pose..." T_B_T_diff = PyKDL.diff(T_B_Trd, velma.getTf("B", "Tr"), 1.0) print T_B_T_diff if T_B_T_diff.vel.Norm() > 0.05 or T_B_T_diff.rot.Norm() > 0.05: releaseRight() (beer_frame, beer_angle) = locateObject("beer") moveRight(beer_frame.p[0] - 0.5 * math.cos(theta), beer_frame.p[1] - 0.5 * math.sin(theta), 0.5 * h_puszki + beer_frame.p[2], beer_angle) #podnosimy reke zeby nie potracic puszki grabRight(0) planAndExecute(q_default_position) exitError(10)
def move_frame(velma, frame): print "Moving right wrist to pose defined in world frame..." if not velma.moveCartImpRight([frame], [3.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5,5,5), PyKDL.Vector(5,5,5)), start_time=0.5): exitError(13) if velma.waitForEffectorRight() != 0: exitError(14) rospy.sleep(0.5)
def leaveHandle(): print "Wyjazd z klamki..." Goal = PyKDL.Vector( doorR_x + 0.1 * math.cos(alfa) + 0.25 * math.sin(alfa), doorR_y + 0.1 * math.sin(alfa) - 0.25 * math.cos(alfa), doorR_z + 0.1) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(0.25 * math.pi + alfa), Goal) if not velma.moveCartImpRight( [T_B_Door], [10.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100, 100, 0.7), PyKDL.Vector(0.7, 0.7, 0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(0.2, 0.2, 1), PyKDL.Vector(1, 1, 1))): exitError(8) if velma.waitForEffectorRight() != 0: print "Velma: :p" rospy.sleep(0.5) resetPoseTol() rospy.sleep(0.5)
def moveToHandle(): print "Ruch do klamki..." DoorR = velma.getTf( "B", "right_door" ) #getTf(frame_from, frame_to); "B"-robot base frame (torso_base); returns: PyKDL.Frame transformation from base frame to target frame doorR_x, doorR_y, doorR_z = DoorR.p DoorL = velma.getTf( "B", "left_door" ) #getTf(frame_from, frame_to); "B"-robot base frame (torso_base); returns: PyKDL.Frame transformation from base frame to target frame doorL_x, doorL_y, doorL_z = DoorL.p position = velma.getLastJointState() Goal = PyKDL.Vector(doorL_x, doorL_y, doorR_z + 0.1) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(0), Goal) if not velma.moveCartImpRight( [T_B_Door], [10.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100, 100, 0.7), PyKDL.Vector(0.7, 0.7, 0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(0.2, 0.2, 1), PyKDL.Vector(1, 1, 1))): exitError(8) if velma.waitForEffectorRight() != 0: print "Velma: Dotykam klamki :p" rospy.sleep(0.5) resetPoseTol() rospy.sleep(0.5)
def setImpedanceRight(velma, imp_p_x, imp_p_y, imp_p_z, imp_r_x, imp_r_y, imp_r_z): if not velma.moveCartImpRight( None, None, None, None, [ PyKDL.Wrench(PyKDL.Vector(imp_p_x, imp_p_y, imp_p_z), PyKDL.Vector(imp_r_x, imp_r_y, imp_r_z)) ], [2], PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.5): exitError(101) if velma.waitForEffectorRight() != 0: exitError(102) rospy.sleep(1)
def move_rotation(velma, rotation): print "Moving right wrist to pose defined in world frame..." T_B_Trd = PyKDL.Frame(rotation, PyKDL.Vector()) if not velma.moveCartImpRight([T_B_Trd], [3.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5,5,5), PyKDL.Vector(5,5,5)), start_time=0.5): exitError(13) if velma.waitForEffectorRight() != 0: exitError(14) rospy.sleep(0.5)
def moveRightHand(rotMatrix, point): T_B_Trd = PyKDL.Frame(rotMatrix, point) if not velma.moveCartImpRight([T_B_Trd], [1.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.5): exitError(13) if velma.waitForEffectorRight() != 0: exitError(14) rospy.sleep(0.5)
def moveForEquilibrium(velma): arm_frame = velma.getTf("B", "Gr") T_Wr_Gr = velma.getTf("Wr", "Gr") if not velma.moveCartImpRight([arm_frame], [0.1], [T_Wr_Gr], [0.1], None, None, PyKDL.Wrench(PyKDL.Vector(5,5,5), PyKDL.Vector(5,5,5)), start_time=0.5): exitError(18) rospy.sleep(0.5) if velma.waitForEffectorRight() != 0: exitError(19) rospy.sleep(0.5)
def move_frame(velma, frame, time_wrist=6, frame_tool=None, time_tool=None): print "Moving right wrist to pose defined in world frame..." # if not velma.moveCartImpRight([frame], [3.0], frame_tool_vector, time_tool_vector, None, None, PyKDL.Wrench(PyKDL.Vector(5,5,5), PyKDL.Vector(5,5,5)), start_time=0.5): twist = PyKDL.Twist(PyKDL.Vector(0.1, 0.1, 0.1), PyKDL.Vector(0.1, 0.1, 0.1)) wr = PyKDL.Wrench(PyKDL.Vector(80, 80, 500), PyKDL.Vector(120, 120, 120)) if not velma.moveCartImpRight([frame], [time_wrist], frame_tool, time_tool, [wr], [time_wrist], PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.5, path_tol=twist): print "wait for three" exitError(13) try: if velma.waitForEffectorRight() != 0: print "wait for effector right" return True except: rospy.sleep(0.5)
def moveCart(B_T): print "Zaczynam ruch nadgarstka" if not velma.moveCartImpRight([B_T], [3.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5,5,5), PyKDL.Vector(5,5,5)), start_time=0.5): exitError(16) if velma.waitForEffectorRight() != 0: exitError(17) rospy.sleep(0.5) print "calculating difference between desiread and reached pose..." T_B_T_diff = PyKDL.diff(B_T, velma.getTf("B", "Tr"), 1.0) print T_B_T_diff if T_B_T_diff.vel.Norm() > 0.05 or T_B_T_diff.rot.Norm() > 0.05: exitError(10)
def pi_controller(self, world): # get current state x = world.get_object_pose(self._object_name) xdot = world.get_object_twist(self._object_name) # calculate error terms e = kdl.diff(x, self._goal_pose) edot = self._goal_twist - xdot # calculate control wrench and apply it control_law = self._p_gain * e + self._d_gain * edot # return kdl.Wrench(control_law.vel, control_law.rot) return kdl.Wrench(control_law.vel, kdl.Vector()) # TODO: fix rot control
def test_transform(self): b = tf2_ros.Buffer() t = TransformStamped() t.transform.translation.x = 1.0 t.transform.rotation = Quaternion(w=0.0, x=1.0, y=0.0, z=0.0) t.header.stamp = builtin_interfaces.msg.Time(sec=2) t.header.frame_id = 'a' t.child_frame_id = 'b' b.set_transform(t, 'eitan_rocks') out = b.lookup_transform('a', 'b', rclpy.time.Time(seconds=2), rclpy.time.Duration(seconds=2)) self.assertEqual(out.transform.translation.x, 1) self.assertEqual(out.transform.rotation.x, 1) self.assertEqual(out.header.frame_id, 'a') self.assertEqual(out.child_frame_id, 'b') v = PyKDL.Vector(1, 2, 3) out = b.transform( tf2_ros.Stamped(v, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.x(), 0) self.assertEqual(out.y(), -2) self.assertEqual(out.z(), -3) f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3)) out = b.transform( tf2_ros.Stamped(f, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.p.x(), 0) self.assertEqual(out.p.y(), -2) self.assertEqual(out.p.z(), -3) # TODO(tfoote) check values of rotation t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) out = b.transform( tf2_ros.Stamped(t, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.vel.x(), 1) self.assertEqual(out.vel.y(), -8) self.assertEqual(out.vel.z(), 2) self.assertEqual(out.rot.x(), 4) self.assertEqual(out.rot.y(), -5) self.assertEqual(out.rot.z(), -6) w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) out = b.transform( tf2_ros.Stamped(w, builtin_interfaces.msg.Time(sec=2), 'a'), 'b') self.assertEqual(out.force.x(), 1) self.assertEqual(out.force.y(), -2) self.assertEqual(out.force.z(), -3) self.assertEqual(out.torque.x(), 4) self.assertEqual(out.torque.y(), -8) self.assertEqual(out.torque.z(), -4)
def resetPoseTol(): print("reseting to real position") tolerance = makeTwist(1, 1, 1, 1, 1, 1) dest_reset = velma.getTf("B", "Wr") velma.moveCartImpRight([dest_reset], [5.0], None, None, [makeWrench(30, 30, 1000, 100, 100, 100)], [1], PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.5, path_tol=tolerance) if velma.waitForEffectorRight() != 0: exitError(17) rospy.sleep(0.5)
def processFeedback(self, feedback): print "feedback", feedback.marker_name, feedback.control_name, feedback.event_type if (feedback.marker_name == self.prefix + '_arm_position_marker') and ( feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK ) and (feedback.control_name == "button"): T_B_Td = pm.fromMsg(feedback.pose) self.p = pm.toMsg(T_B_Td) self.velma.moveCartImp(self.prefix, [T_B_Td], [5.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.5)
def test_transform(self): b = tf2_ros.Buffer() t = TransformStamped() t.transform.translation.x = 1 t.transform.rotation.x = 1 t.header.stamp = rospy.Time(2.0) t.header.frame_id = 'a' t.child_frame_id = 'b' b.set_transform(t, 'eitan_rocks') out = b.lookup_transform('a', 'b', rospy.Time(2.0), rospy.Duration(2.0)) self.assertEqual(out.transform.translation.x, 1) self.assertEqual(out.transform.rotation.x, 1) self.assertEqual(out.header.frame_id, 'a') self.assertEqual(out.child_frame_id, 'b') v = PyKDL.Vector(1, 2, 3) out = b.transform(tf2_ros.Stamped(v, rospy.Time(2), 'a'), 'b') self.assertEqual(out.x(), 0) self.assertEqual(out.y(), -2) self.assertEqual(out.z(), -3) f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3)) out = b.transform(tf2_ros.Stamped(f, rospy.Time(2), 'a'), 'b') print(out) self.assertEqual(out.p.x(), 0) self.assertEqual(out.p.y(), -2) self.assertEqual(out.p.z(), -3) # TODO(tfoote) check values of rotation t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) out = b.transform(tf2_ros.Stamped(t, rospy.Time(2), 'a'), 'b') self.assertEqual(out.vel.x(), 1) self.assertEqual(out.vel.y(), -8) self.assertEqual(out.vel.z(), 2) self.assertEqual(out.rot.x(), 4) self.assertEqual(out.rot.y(), -5) self.assertEqual(out.rot.z(), -6) w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) out = b.transform(tf2_ros.Stamped(w, rospy.Time(2), 'a'), 'b') self.assertEqual(out.force.x(), 1) self.assertEqual(out.force.y(), -2) self.assertEqual(out.force.z(), -3) self.assertEqual(out.torque.x(), 4) self.assertEqual(out.torque.y(), -8) self.assertEqual(out.torque.z(), -4)
def leaveCabinet(): print "Oddalenie od szafki..." DoorR = velma.getTf("B", "right_door") #getTf(frame_from, frame_to); "B"-robot base frame (torso_base); returns: PyKDL.Frame transformation from base frame to target frame doorR_x, doorR_y, doorR_z = DoorR.p DoorL = velma.getTf("B", "left_door") #getTf(frame_from, frame_to); "B"-robot base frame (torso_base); returns: PyKDL.Frame transformation from base frame to target frame doorL_x, doorL_y, doorL_z = DoorL.p Goal = PyKDL.Vector(1/3*doorL_x+2/3*doorR_x-0.4,1/3*doorL_y+2/3*doorR_y-0.2,doorR_z+0.1-0.25) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(0), Goal) if not velma.moveCartImpRight([T_B_Door], [5.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100,100,0.7), PyKDL.Vector(0.7,0.7,0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(1,1,1),PyKDL.Vector(1,1,1))): exitError(8) if velma.waitForEffectorRight() != 0: exitError(9) rospy.sleep(0.5)
def moveInCartImpMode(velma, T_B_dest, tol=10): if not velma.moveCartImpRight( [T_B_dest], [5.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.1, path_tol=PyKDL.Twist(PyKDL.Vector(tol, tol, tol), PyKDL.Vector(tol, tol, tol))): exitError(8) if velma.waitForEffectorRight() != 0: if not velma.moveCartImpRightCurrentPos(start_time=0.01): exitError(9) return False else: return True
def moveToCimp(): # moving tool frame to grip? print "Moving the right tool and equilibrium pose from 'wrist' to 'grip' frame..." T_B_Wr = velma.getTf("B", "Wr") T_Wr_Gr = velma.getTf("Wr", "Gr") if not velma.moveCartImpRight([T_B_Wr * T_Wr_Gr], [0.1], [T_Wr_Gr], [0.1], None, None, PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.5): exitError(18) if velma.waitForEffectorRight() != 0: exitError(19) print "the right tool is now in 'grip' pose" rospy.sleep(0.5) print "Switch to cart_imp mode (no trajectory)..." if not velma.moveCartImpRightCurrentPos(start_time=0.2): exitError(10) if velma.waitForEffectorRight() != 0: exitError(11) rospy.sleep(0.5)
def setImpedance(): print "Set impedance to (10,10,1000,150,150,150) in tool frame." imp_list = [ makeWrench(1000, 1000, 1000, 150, 150, 150), makeWrench(500, 500, 1000, 150, 150, 150), makeWrench(100, 100, 1000, 150, 150, 150), makeWrench(10, 10, 1000, 150, 150, 150) ] if not velma.moveCartImpRight(None, None, None, None, imp_list, [0.5, 1.0, 1.5, 2.0], PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.5): exitError(16) if velma.waitForEffectorRight() != 0: exitError(17)
def main(): b = tf2_ros.Buffer() t = TransformStamped() t.transform.translation.x = 1 t.transform.rotation.x = 1 t.header.stamp = rospy.Time(2.0) t.header.frame_id = 'a' t.child_frame_id = 'b' b.set_transform(t, 'eitan_rocks') print b.lookup_transform('a', 'b', rospy.Time(2.0), rospy.Duration(2.0)) v = PyKDL.Vector(1, 2, 3) print b.transform(tf2_ros.Stamped(v, rospy.Time(2), 'a'), 'b') f = PyKDL.Frame(PyKDL.Rotation.RPY(1, 2, 3), PyKDL.Vector(1, 2, 3)) print b.transform(tf2_ros.Stamped(f, rospy.Time(2), 'a'), 'b') t = PyKDL.Twist(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) print b.transform(tf2_ros.Stamped(t, rospy.Time(2), 'a'), 'b') w = PyKDL.Wrench(PyKDL.Vector(1, 2, 3), PyKDL.Vector(4, 5, 6)) print b.transform(tf2_ros.Stamped(w, rospy.Time(2), 'a'), 'b')
def moveToInteractiveCursor(velma): T_Wo_test = velma.getTf("Wo", "example_frame") if not velma.moveCartImpRightCurrentPos(start_time=0.2): exitError(8) if velma.waitForEffectorRight() != 0: exitError(9) if not velma.moveCartImpRight([T_Wo_test], [3.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.5): exitError(8) if velma.waitForEffectorRight() != 0: exitError(9) rospy.sleep(0.5) T_B_T_diff = PyKDL.diff(T_Wo_test, velma.getTf("B", "Tr"), 1.0) if T_B_T_diff.vel.Norm() > 0.05 or T_B_T_diff.rot.Norm() > 0.05: exitError(10)
def leaveCabinet(): print "Oddalenie od szafki..." Goal = PyKDL.Vector(1 / 3 * doorL_x + 2 / 3 * doorR_x - 0.4, 1 / 3 * doorL_y + 2 / 3 * doorR_y - 0.2, doorR_z + 0.1 - 0.25) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(0), Goal) if not velma.moveCartImpRight( [T_B_Door], [5.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100, 100, 0.7), PyKDL.Vector(0.7, 0.7, 0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(1, 1, 1), PyKDL.Vector(1, 1, 1))): exitError(8) if velma.waitForEffectorRight() != 0: exitError(9) rospy.sleep(0.5)
def moveToHandle(): print "Ruch do klamki..." Goal = PyKDL.Vector(doorL_x, doorL_y, doorR_z + 0.1) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(alfa), Goal) if not velma.moveCartImpRight( [T_B_Door], [10.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100, 100, 0.7), PyKDL.Vector(0.7, 0.7, 0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(0.2, 0.2, 1), PyKDL.Vector(1, 1, 1))): exitError(8) if velma.waitForEffectorRight() != 0: print "Velma: Dotykam klamki :p" rospy.sleep(0.5) resetPoseTol() rospy.sleep(0.5)
def makeWrench(force, torque): f = PyKDL.Vector(force[0], force[1], force[2]) t = PyKDL.Vector(torque[0], torque[1], torque[2]) return PyKDL.Wrench(f, t)
"right_arm_5_joint", "right_arm_6_joint", "left_arm_0_joint", "left_arm_1_joint", "left_arm_2_joint", "left_arm_3_joint", "left_arm_4_joint", "left_arm_5_joint", "left_arm_6_joint" ] init_q = [ 0, 0, -1.57, 1.57, 1.57, 0, -1.57, 0, 0, 1.57, -1.57, -1.57, 0, 1.57, 0 ] velma.moveJoint(init_q, init_q_joint_names, 5.0, start_time=0.5, position_tol=15.0 / 180.0 * math.pi) velma.waitForJoint() max_wr = PyKDL.Wrench(PyKDL.Vector(10, 10, 10), PyKDL.Vector(4, 4, 4)) print "moving right tool to wrist frame..." T_B_Wr = velma.getTf('B', 'Wr') velma.moveCartImpRight([T_B_Wr], [0.5], [PyKDL.Frame()], [0.5], None, None, max_wr, start_time=0.5) result = velma.waitForEffectorRight() if result != CartesianTrajectoryResult.SUCCESSFUL: print result raise Exception() print "moving left tool to wrist frame..." T_B_Wl = velma.getTf('B', 'Wl')
def makeWrench(lx,ly,lz,rx,ry,rz): return PyKDL.Wrench(PyKDL.Vector(lx,ly,lz), PyKDL.Vector(rx,ry,rz))
jarFrame = velma.getTf("B", "jar") jarX, jarY, jarZ = jarFrame.p actual_pos = velma.getLastJointState() rotZ = actual_pos[1]['torso_0_joint'] + math.pi print("podchodze do celu") Rot = PyKDL.Rotation.RotZ(rotZ) P2_global = PyKDL.Vector(jarX - 0.25, jarY, jarZ + 0.1) T_B_Trd = PyKDL.Frame(Rot, P2_global) velma.moveCartImpLeft([T_B_Trd], [4.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)), start_time=0.5) if velma.waitForEffectorLeft() != 0: exitError(17) rospy.sleep(0.5) #close fingers left hand and grab jar print("Taking jar.") fin_angle = 0.43 * math.pi dest_q = [fin_angle, fin_angle, fin_angle, 0] velma.moveHandLeft(dest_q, [1, 1, 1, 1], [2000, 2000, 2000, 2000], 1000, hold=True) if velma.waitForHandLeft() != 0: exitError(6) rospy.sleep(0.5)
def openDoor(): DoorR = velma.getTf("B", "right_door") #getTf(frame_from, frame_to); "B"-robot base frame (torso_base); returns: PyKDL.Frame transformation from base frame to target frame doorR_x, doorR_y, doorR_z = DoorR.p DoorL = velma.getTf("B", "left_door") #getTf(frame_from, frame_to); "B"-robot base frame (torso_base); returns: PyKDL.Frame transformation from base frame to target frame doorL_x, doorL_y, doorL_z = DoorL.p Cabinet = velma.getTf("B", "cabinet") cabX, cabY, cabZ = Cabinet.p position = velma.getLastJointState() kat_szafki=math.atan2(cabY,cabX) print "Otwieranie drzwi: ruch (1)..." Goal = PyKDL.Vector(cabX-math.cos(alfa),cabY-math.sin(alfa),doorR_z+0.1) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(math.pi*0.4+alfa), Goal) if not velma.moveCartImpRight([T_B_Door], [5.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100,100,0.7), PyKDL.Vector(0.7,0.7,0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(0.2,0.2,1),PyKDL.Vector(1,1,1))): exitError(8) if velma.waitForEffectorRight() != 0: print "Velma: otwieram drzwi" rospy.sleep(0.5) resetPoseTol() rospy.sleep(0.5) print "Otwieranie drzwi: ruch (2)..." Goal = PyKDL.Vector(cabX-2.0*math.cos(alfa)+1.5*math.sin(alfa),cabY-2.0*math.sin(alfa)-1.5*math.cos(alfa),doorR_z+0.1) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(math.pi*0.8+alfa), Goal) if not velma.moveCartImpRight([T_B_Door], [5.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100,100,0.7), PyKDL.Vector(0.7,0.7,0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(0.2,0.2,1),PyKDL.Vector(1,1,1))): exitError(8) if velma.waitForEffectorRight() != 0: print "Velma: otwieram drzwi" rospy.sleep(0.5) resetPoseTol() rospy.sleep(0.5) print "Otwieranie drzwi: ruch (3)..." Goal = PyKDL.Vector(cabX-1.5*math.cos(alfa)+3.0*math.sin(alfa),cabY-1.5*math.sin(alfa)-3.0*math.cos(alfa),doorR_z+0.1) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(math.pi+alfa), Goal) if not velma.moveCartImpRight([T_B_Door], [5.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100,100,0.7), PyKDL.Vector(0.7,0.7,0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(0.2,0.2,1),PyKDL.Vector(1,1,1))): exitError(8) if velma.waitForEffectorRight() != 0: print "Velma: otwieram drzwi" rospy.sleep(0.5) resetPoseTol() rospy.sleep(0.5) print "Otwieranie drzwi: ruch (4)..." Goal = PyKDL.Vector(cabX-1.0*math.cos(alfa)+2.0*math.sin(alfa),cabY-1.0*math.sin(alfa)-2.0*math.cos(alfa),doorR_z+0.1) T_B_Door = PyKDL.Frame(PyKDL.Rotation.RotZ(0.6*math.pi+alfa), Goal) if not velma.moveCartImpRight([T_B_Door], [5.0], None, None, None, None, PyKDL.Wrench(PyKDL.Vector(100,100,0.7), PyKDL.Vector(0.7,0.7,0.7)), start_time=0.5, path_tol=PyKDL.Twist(PyKDL.Vector(0.2,0.2,1),PyKDL.Vector(1,1,1))): exitError(8) if velma.waitForEffectorRight() != 0: print "Velma: otwieram drzwi" rospy.sleep(0.5) resetPoseTol() rospy.sleep(0.5)