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 initialize_robot():
'''
Function initializes robot in cart_imp mode
'''
global velma
velma = VelmaInterface()
if not velma.waitForInit(timeout_s=10.0):
print "Could not initialize VelmaInterface\n"
exitError(1)
print "Initialization ok!\n"
diag = velma.getCoreCsDiag()
if not diag.motorsReady():
print "Motors must be homed and ready to use for this test."
exitError(1)
print "Switch to cart_imp mode (no trajectory)..."
if not velma.moveCartImpRightCurrentPos(start_time=0.2):
exitError(8)
if velma.waitForEffectorRight() != 0:
exitError(9)
diag = velma.getCoreCsDiag()
if not diag.inStateCartImp():
print "The core_cs should be in cart_imp state, but it is not"
exitError(3)
def getAdjFrame(velma, r):
global rotation
handlePos = velma.getTf("Wo", "handle")
handleTwinPos = velma.getTf("Wo", "handle_twin")
rotPointPos = velma.getTf("Wo", "rot_point")
dx = handleTwinPos.p[0] - handlePos.p[0]
dy = handleTwinPos.p[1] - handlePos.p[1]
l = math.sqrt(math.pow(dx, 2) + math.pow(dy, 2))
vx = (r / l) * (dx * math.cos(math.pi / 2) - dy * math.sin(math.pi / 2))
vy = (r / l) * (dx * math.sin(math.pi / 2) + dy * math.cos(math.pi / 2))
rot = math.atan2(vy, vx)
if (rotation == 1):
frameRot = PyKDL.Rotation.RPY(0, math.pi / 2, 0) * PyKDL.Rotation.RPY(
-rot, 0, 0) #1st rotation
elif (rotation == 2):
frameRot = PyKDL.Rotation.RPY(math.pi,
-math.pi / 2, 0) * PyKDL.Rotation.RPY(
rot, 0, 0) #2nd rotation
else:
exitError(0)
wx = handlePos.p[0] - vx
wy = handlePos.p[1] - vy
wz = handlePos.p[2]
frameVec = PyKDL.Vector(wx, wy, wz)
frame = PyKDL.Frame(frameRot, frameVec)
return frame
def move_joint_impedance(joints):
global velma
global planner
# switch to joint impedance mode
switch_to_joint_impedance_mode()
print "Planning motion to the goal position using set of all joints..."
print "Moving to valid position, using planned trajectory."
goal_constraint_1 = qMapToConstraints(
joints, 0.01, group=velma.getJointGroup("impedance_joints"))
for i in range(10):
rospy.sleep(0.2)
js = velma.getLastJointState()
print "Planning (try", i, ")..."
traj = planner.plan(js[1], [goal_constraint_1],
"impedance_joints",
num_planning_attempts=10,
max_velocity_scaling_factor=0.15,
planner_id="RRTConnect")
if traj == None:
continue
print "Executing trajectory..."
if not velma.moveJointTraj(traj,
start_time=0.5,
position_tol=10.0 / 180.0 * math.pi,
velocity_tol=10.0 / 180.0 * math.pi):
exitError(5)
if velma.waitForJoint() == 0:
break
else:
print "The trajectory could not be completed, retrying..."
continue
rospy.sleep(0.5)
js = velma.getLastJointState()
if not isConfigurationClose(joints, js[1]):
print "Error nr 6 - Robot position in joint state is not close enough!"
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 plan_and_move(q_map_goal):
'''
Function plans robot's movement to given goal (values for joints) and executes it.
'''
print "Planning motion to the goal."
print "Moving to valid position, using planned trajectory."
goal_constraint_1 = qMapToConstraints(
q_map_goal, 0.01, group=velma.getJointGroup("impedance_joints"))
for i in range(15):
rospy.sleep(0.5)
js = velma.getLastJointState()
print "Planning (try", i, ")..."
traj = p.plan(js[1], [goal_constraint_1],
"impedance_joints",
num_planning_attempts=10,
max_velocity_scaling_factor=0.15,
planner_id="RRTConnect")
if traj == None:
continue
print "Executing trajectory..."
if not velma.moveJointTraj(traj,
start_time=0.5,
position_tol=10.0 / 180.0 * math.pi,
velocity_tol=10.0 / 180.0 * math.pi):
exitError(11)
if velma.waitForJoint() == 0:
break
else:
print "The trajectory could not be completed, retrying..."
continue
rospy.sleep(0.5)
js = velma.getLastJointState()
if not isConfigurationClose(q_map_goal, js[1]):
exitError(12)
def move_joint(velma, planner, pose):
print "Moving to valid position, using planned trajectory."
goal_constraint_1 = qMapToConstraints(
pose, 0.01, group=velma.getJointGroup("impedance_joints"))
for i in range(5):
print 'move_joint iteration ' + str(i)
rospy.sleep(0.5)
js = velma.getLastJointState()
print "Planning (try", i, ")..."
traj = planner.plan(js[1], [goal_constraint_1],
"impedance_joints",
num_planning_attempts=10,
max_velocity_scaling_factor=0.15,
planner_id="RRTConnect")
if traj == None:
continue
print "Executing trajectory..."
#print traj
if not velma.moveJointTraj(traj,
start_time=0.5,
position_tol=10.0 / 180.0 * math.pi,
velocity_tol=10.0 / 180.0 * math.pi):
print 'Error in moveJointTraj'
exitError(9)
if velma.waitForJoint() == 0:
break
else:
print "The trajectory could not be completed, retrying..."
continue
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 grabRight(catching):
modeImp()
dest_q = [
75.0 / 180.0 * math.pi, 75.0 / 180.0 * math.pi, 75.0 / 180.0 * math.pi,
0
]
print "Taking a hold"
velma.moveHandRight(dest_q, [1, 1, 1, 1], [2000, 2000, 2000, 2000],
1000,
hold=True)
if velma.waitForHandRight() != 0:
exitError(8)
rospy.sleep(0.5)
if not isHandConfigurationClose(velma.getHandRightCurrentConfiguration(),
dest_q):
if (catching == 1):
print "Grabbing succesfull. Commencing movement to the drop zone"
else:
print "Could not take a hold"
exitError(8)
else:
if (catching == 1):
print "Failure to take a hold. Returning to default position"
releaseRight()
moveRight(0.6 * beerFrame.p[0], 0.6 * beerFrame.p[1],
0.4 * h_puszki + beerFrame.p[2], beerAngle)
planAndExecute(q_default_position)
def planAndExecute(q_dest):
"""!
Fukcja planuje oraz wykonuje ruch do zadanej pozycji stawow
@param q_dest slownik: Slownik {nazwa_stawu:pozycja_docelowa} zawierajacy docelowe pozycje stawow.
"""
goal_constraint = qMapToConstraints(q_dest, 0.01, group=velma.getJointGroup("impedance_joints"))
for i in range(20):
rospy.sleep(0.5)
js = velma.getLastJointState()
print "Planning (try", i, ")..."
traj = p.plan(js[1], [goal_constraint], "impedance_joints", max_velocity_scaling_factor=0.4,
max_acceleration_scaling_factor=0.3, planner_id="RRTConnect")
if traj == None:
continue
print "Executing trajectory..."
if not velma.moveJointTraj(traj, start_time=0.1, position_tol=10.0/180.0*math.pi):
exitError(5)
if velma.waitForJoint() == 0:
break
else:
print "The trajectory could not be completed, retrying..."
continue
rospy.sleep(0.5)
js = velma.getLastJointState()
if not isConfigurationClose(q_dest, js[1]):
exitError(6)
def moveWristToPos(T_B_Trd,
move_time=3,
tol=PyKDL.Twist(PyKDL.Vector(0.04, 0.04, 0.04),
PyKDL.Vector(0.04, 0.04, 0.04))):
"""!
Ruch nadgarstka do zadanej pozycji.
@param T_B_Trd PyKDL.Frame: Pozycja nadgarstka robota wzgledem ukladu wspolrzednych bazy.
"""
T_Wr_Gr = velma.getTf("Wr", "Gr")
if not velma.moveCartImpRight(
[T_B_Trd],
[move_time],
None,
None,
None,
None,
PyKDL.Wrench(PyKDL.Vector(5, 5, 5), PyKDL.Vector(5, 5, 5)),
start_time=0.01,
path_tol=tol,
):
exitError(8)
if velma.waitForEffectorRight() != 0:
if not velma.moveCartImpRightCurrentPos(start_time=0.01):
exitError(98)
return 0
else:
return 1
def impedStearing(T_B_Trd,imp_list,pt): # cart
global move_time
print("Rozpoczecie sterowania impendacyjnego---------------------------")
# This test/tutorial executes simple impedance commands in Cartesian impedance mode.
# To see the tool frame add 'tf' in rviz and enable 'right_arm_tool' frame.
# At every state switch to cart_imp, the tool frames are reset.
# Also, the tool impedance parameters are reset to 1500N/m in every
# direction for linear stiffness and to 150Nm/rad in every direction for angular
# stiffness, i.e. (1500,1500,1500,150,150,150)."
"""TEST WALNIECIA W SZAFKE"""
t=countTime(T_B_Trd)
actual_gripper_position = velma.getTf("B", "Tr") #aktualna pozycja chwytaka
print("Moving right wrist to pose defined in world frame...")
"""Tak tutaj dalem 3 zamiast move_time*t bo dla malych ruchow wywalalo errora bo za maly czas wyliczalo"""
if not velma.moveCartImpRight([T_B_Trd], [3], None, None, imp_list, [0.5], max_wrench=makeWrench([5,5,5], [5,5,5]),
start_time=0.01, path_tol=PyKDL.Twist(PyKDL.Vector(pt,pt,pt), PyKDL.Vector(pt,pt,pt))):
exitError(13)
if velma.waitForEffectorRight() != 0: #zglaszane jak chwytak nie moze osiagnac zadanej pozycji
return actual_gripper_position.p[0] , actual_gripper_position.p[1], actual_gripper_position.p[2]
print("Pose reached, there was no collision")
return actual_gripper_position.p[0] , actual_gripper_position.p[1], actual_gripper_position.p[2]
def impedMove(T_B_Trd, imp_list, pt, tm=3): # cart
print "Rozpoczecie sterowania impendacyjnego---------------------------"
# actual_gripper_position = velma.getTf("B", "Tr") # aktualna pozycja chwytaka
print "Moving right wrist to pose defined in world frame..."
my_wrench = makeWrench([5, 5, 5], [5, 5, 5])
my_twist = PyKDL.Twist(PyKDL.Vector(pt, pt, pt),
PyKDL.Vector(pt, pt, pt))
rospy.sleep(0.5)
print(len(imp_list))
"""Tak tutaj dalem 3 zamiast move_time*t bo dla malych ruchow wywalalo errora bo za maly czas wyliczalo"""
if not velma.moveCartImpRight([T_B_Trd], [tm],
None,
None,
imp_list, [0.5],
max_wrench=my_wrench,
start_time=0.01,
path_tol=my_twist):
exitError(13)
if velma.waitForEffectorRight(
) != 0: # zglaszane jak chwytak nie moze osiagnac zadanej pozycji
print("I think it worked?")
return #actual_gripper_position.p[0], actual_gripper_position.p[1], actual_gripper_position.p[2]
print "Pose reached, there was no collision"
return #actual_gripper_position.p[0], actual_gripper_position.p[1], actual_gripper_position.p[2]
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 planAndExecute(q_dest):
print "Planning motion to the goal position using set of all joints..."
print "Moving to valid position, using planned trajectory."
goal_constraint = qMapToConstraints(
q_dest, 0.01, group=velma.getJointGroup("impedance_joints"))
for i in range(5):
rospy.sleep(0.5)
js = velma.getLastJointState()
print "Planning (try", i, ")..."
traj = p.plan(js[1], [goal_constraint],
"impedance_joints",
max_velocity_scaling_factor=0.15,
planner_id="RRTConnect")
if traj == None:
continue
print "Executing trajectory..."
if not velma.moveJointTraj(traj, start_time=0.5):
exitError(5)
if velma.waitForJoint() == 0:
break
else:
print "The trajectory could not be completed, retrying..."
continue
rospy.sleep(0.5)
js = velma.getLastJointState()
if not isConfigurationClose(q_dest, js[1]):
exitError(6)
def planAndExecute(velma, q_dest):
# type: (object, object, object) -> object
goal_constraint = qMapToConstraints(
q_dest, 0.01, group=velma.getJointGroup("impedance_joints"))
for i in range(10):
rospy.sleep(0.5)
js = velma.getLastJointState()
#print "Planning (try", i, ")..."
traj = p.plan(js[1], [goal_constraint],
"impedance_joints",
max_velocity_scaling_factor=0.15,
planner_id="RRTConnect")
if traj == None:
continue
if not velma.moveJointTraj(traj, start_time=0.5):
exitError(5)
if velma.waitForJoint() == 0:
break
else:
print "The trajectory could not be completed, retrying..."
continue
rospy.sleep(0.5)
js = velma.getLastJointState()
if not isConfigurationClose(q_dest, js[1]):
exitError(6)
def makeGrip():
print "Ukladanie dloni do chwytu klamki..."
dest_q=[0.6*math.pi,0.6*math.pi,0.6*math.pi,math.pi]
velma.moveHandRight(dest_q, [1,1,1,1], [2000,2000,2000,2000], 1000, hold=True)
if velma.waitForHandRight() != 0:
exitError(8)
rospy.sleep(0.5)
def toJnp():
print "Switch to jnt_imp mode (no trajectory)..."
velma.moveJointImpToCurrentPos(start_time=0.5)
error = velma.waitForJoint()
if error != 0:
print "The action should have ended without error, but the error code is", error
exitError(3)
def openFingers():
print "Palce..."
dest_q=[0,0,0,0]
velma.moveHandRight(dest_q, [1,1,1,1], [2000,2000,2000,2000], 1000, hold=True)
if velma.waitForHandRight() != 0:
exitError(8)
rospy.sleep(0.5)
def right_gripper_action(dest_q,isBeer):
print "move right:", dest_q
velma.moveHandRight(dest_q, [1,1,1,1], [2000,2000,2000,2000], 1000, hold=True)
if velma.waitForHandRight() != 0:
exitError(6)
rospy.sleep(0.5)
checkRight(dest_q,isBeer)
def toCart():
print "Switch to cart_imp mode (no trajectory)..."
if not velma.moveCartImpRightCurrentPos(start_time=0.2):
exitError(8)
if velma.waitForEffectorRight() != 0:
exitError(9)
rospy.sleep(0.5)
def moveHead(q_dest):
velma.moveHead(q_dest, 3.0, start_time=0.5)
if velma.waitForHead() != 0:
exitError(4)
rospy.sleep(0.5)
if not isHeadConfigurationClose( velma.getHeadCurrentConfiguration(), q_dest, 0.1 ):
exitError(5)
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 rotateTorso(velma, torso_angle, q_map):
q_map['torso_0_joint'] = torso_angle
velma.moveJoint(q_map, 4.0, start_time=0.5, position_tol=15.0/180.0*math.pi)
error = velma.waitForJoint()
if error != 0:
print "The action should have ended without error, but the error code is", error
exitError(10)
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 moveJnt(q_map):
modeJnt()
print "Moving to set position (jimp)"
velma.moveJoint(q_map, 1, start_time=0.5, position_tol=0, velocity_tol=0)
error = velma.waitForJoint()
if error != 0:
print "The action should have ended without error, but the error code is", error
exitError(6)
def resetPoseTol():
print "Resetowanie pozycji koncowki..."
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 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 closeFingers(): dest_q = [90.0/180.0*math.pi,90.0/180.0*math.pi,90.0/180.0*math.pi,0] print "move right:", dest_q velma.moveHandRight(dest_q, [1,1,1,1], [2000,2000,2000,2000], 1000, hold=True) if velma.waitForHandRight() != 0: exitError(8) rospy.sleep(0.5) return dest_q
def moveRightGripper(dest_q): # jnp
print("fold right gripper's fingers")
velma.moveHandRight(dest_q, [1,1,1,1], [2000,2000,2000,2000], 1000, hold=True)
if velma.waitForHandRight() != 0:
exitError(10)
rospy.sleep(0.5)
if not isHandConfigurationClose( velma.getHandRightCurrentConfiguration(), dest_q):
exitError(11)
