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)
