def test_matrix_rpy(self):
for mat, rpy in [
(mod.matrixToTuple(np.identity(4)), (0, 0, 0, 0, 0, 0)),
(mod.matrixToTuple(-np.identity(4)), (0, 0, 0, -np.pi, 0, -np.pi)),
]:
np.testing.assert_allclose(rpy, mod.matrixToRPY(mat))
# np.testing.assert_allclose(mat, mod.RPYToMatrix(rpy))
np.testing.assert_allclose(rpy,
mod.matrixToRPY(mod.RPYToMatrix(rpy)))
def test_rotate(self):
for axis, angle, mat in [
('x', np.pi, ((1, 0, 0, 0), (0, -1, 0, 0), (0, 0, -1, 0),
(0, 0, 0, 1))),
('y', np.pi, ((-1, 0, 0, 0), (0, 1, 0, 0), (0, 0, -1, 0),
(0, 0, 0, 1))),
('z', np.pi, ((-1, 0, 0, 0), (0, -1, 0, 0), (0, 0, 1, 0),
(0, 0, 0, 1))),
]:
self.assertEqual(mat, mod.rotate(axis, angle))
def test_quat_mat(self):
for quat, mat in [
((0, 0, 0, 1), np.identity(3)),
((0, 0, 1, 0), ((-1, 0, 0), (0, -1, 0), (0, 0, 1))),
((0, -0.5, 0, 0.5), ((0.5, 0, -0.5), (0, 1, 0), (0.5, 0,
0.5))),
]:
self.assertEqual(mat, mod.quaternionToMatrix(quat))
def updateFeetCollision():
''' Update the box for the feet '''
wMcomr = matrixToTuple(matrix(dyn.rf.value)*arMcomr) # world_M_ankleRight * anklRight_M_comr
wMcoml = matrixToTuple(matrix(dyn.lf.value)*alMcoml) # wMar*arMcomr
wMcomr_XYZ_RPY = matrixToRPY( wMcomr ) # com of RF box in (x,y,z,r,p,y) format
wMcoml_XYZ_RPY = matrixToRPY( wMcoml ) # com of LF box in (x,y,z,r,p,y) format
robot.viewer.updateElementConfig('boxRF', wMcomr_XYZ_RPY) # View the right foot box
robot.viewer.updateElementConfig('boxLF', wMcoml_XYZ_RPY) # View the right foot box
fclRf.setBoxTransf(wMcomr) # Set pose of RF box (for collision detection)
fclLf.setBoxTransf(wMcoml) # Set pose of LF box (for collision detection)
''' Show contact points '''
# Prf = fclRf.contactPoints.value # Contact points 'Pc' in the world frame 'w'
# Plf = fclLf.contactPoints.value # Contact points 'Pc' in the world frame 'w'
Prf = gridRf.contactPoints.value # Contact points 'Pc' in the world frame 'w'
Plf = gridLf.contactPoints.value # Contact points 'Pc' in the world frame 'w'
for i in range(len(Prf)): # Show the contact points for the right foot
name = 'point'+str(i+1)
robot.viewer.updateElementConfig(name, [Prf[i][0],Prf[i][1],Prf[i][2],0,0,0])
for i in range(len(Plf)): # Show the contact points for the left foot
name = 'point'+str(i+1+len(Prf))
robot.viewer.updateElementConfig(name, [Plf[i][0],Plf[i][1],Plf[i][2],0,0,0])
for i in range(NPt-len(Plf)-len(Prf)): # Hide the points that are not in contact
name = 'point'+str(i+1+len(Prf)+len(Plf))
robot.viewer.updateElementConfig(name, [0,0,-1,0,0,0])
# ------------------------------------------------------------------------------
dyn.rf.recompute(0)
dyn.lf.recompute(0)
# --- Definitions for the feet sole ---
soleRF = ((0.11,-0.08,-0.08,0.11),(-0.065,-0.065,0.045,0.045),(-0.105,-0.105,-0.105,-0.105))
soleLF = ((0.11,-0.08,-0.08,0.11),(0.065,0.065,-0.045,-0.045),(-0.105,-0.105,-0.105,-0.105))
arPcomr = matrix(( average(soleRF[0]), average(soleRF[1]), -0.105 + 0.012 )).T
alPcoml = matrix(( average(soleLF[0]), average(soleLF[1]), -0.105 + 0.012 )).T
arMcomr = bmat([ [mat(eye(3)),arPcomr], [mat(zeros(3)),mat(1.)] ])
alMcoml = bmat([ [mat(eye(3)),alPcoml], [mat(zeros(3)),mat(1.)] ])
wMcomr = matrixToTuple(matrix(dyn.rf.value)*arMcomr) # wMar*arMcomr
wMcoml = matrixToTuple(matrix(dyn.lf.value)*alMcoml) # wMar*arMcomr
wMcomr_XYZ_RPY = matrixToRPY( wMcomr ) # com of right foot box in (x,y,z,r,p,y) format
wMcoml_XYZ_RPY = matrixToRPY( wMcoml ) # com of left foot box in (x,y,z,r,p,y) format
# --- Collision detection with the floor ----
fclRf = FclBoxMeshCollision('fclRf')
fclRf.setMinDistance(0.002)
fclRf.setBoxDimensions((0.23, 0.135, 0.024))
fclRf.setBoxTransf(wMcomr)
fclRf.setMeshFile('plane2mCentered_005.obj')
fclRf.setMeshPose((0,0,0,0,0,0))
fclLf = FclBoxMeshCollision('fclLf')
fclLf.setMinDistance(0.002)
fclLf.setBoxDimensions((0.23, 0.135, 0.024))
fclLf.setBoxTransf(wMcoml)
def supervision():
global state, tool, robot, solver, pos_err_des, gainMax, gainMin, goal, p72tohole, m2sig
if state == 0:
robot.mTasks['lh'].feature.error.recompute(robot.device.control.time)
if linalg.norm(array(
robot.mTasks['lh'].feature.error.value)[0:3]) < pos_err_des:
openGrippers(robot, solver)
#state = 1000 #needed if you want to make the robot wait
state = 1
print "time = " + str(robot.device.control.time)
if state == 1:
robot.device.state.recompute(robot.device.control.time)
if linalg.norm(
array([
robot.device.state.value[28] -
robot.mTasks['posture'].ref[28],
robot.device.state.value[35] -
robot.mTasks['posture'].ref[35]
])) < 0.003:
robot.device.viewer.updateElementConfig('TwoHandTool', tool)
closeGrippers(robot, solver)
state += 1
print "time = " + str(robot.device.control.time)
if state == 2:
robot.device.state.recompute(robot.device.control.time)
if linalg.norm(
array([
robot.device.state.value[28] -
robot.mTasks['posture'].ref[28],
robot.device.state.value[35] -
robot.mTasks['posture'].ref[35]
])) < 0.003:
print "Goal: " + str(array(goal.value))
#here there is the plug
screw_2ht(robot, solver, tool, P72, goal, gainMax, gainMin)
#write_pos_py("/opt/grx3.0/HRP2LAAS/script/airbus_robot/",robot.device.state.value[6:36])
state += 1
print "time = " + str(robot.device.control.time)
if state >= 3 and state <= 19:
robot.mTasks['screw'].feature.error.recompute(
robot.device.control.time)
if linalg.norm(array(
robot.mTasks['screw'].feature.error.value)) < pos_err_des:
print "state = " + str(state)
if state < len(p72tohole) + 2:
p72togoal.sin2.value = p72tohole[state - 2]
print "Goal: " + str(array(goal))
#screw_2ht(robot,solver,tool,goal,gainMax, gainMin)
goal.recompute(robot.device.control.time)
robot.device.viewer.updateElementConfig(
'goal1', vectorToTuple(array(matrixToRPY(goal.value))))
if state == len(p72tohole) + 2:
state = 20
else:
state += 1
print "time = " + str(robot.device.control.time)
if state == 20:
robot.mTasks['screw'].feature.error.recompute(
robot.device.control.time)
if linalg.norm(array(
robot.mTasks['screw'].feature.error.value)[0:3]) < pos_err_des:
get_2ht(robot, solver, tool, gainMax, gainMin)
state += 1
print "time = " + str(robot.device.control.time)
if state == 21:
robot.mTasks['lh'].feature.error.recompute(robot.device.control.time)
if linalg.norm(array(
robot.mTasks['lh'].feature.error.value)[0:3]) < pos_err_des:
openGrippers(robot, solver)
#state = 8000 #needed if you want to make the robot wait
state = 22
print "time = " + str(robot.device.control.time)
if state == 22:
robot.device.state.recompute(robot.device.control.time)
if linalg.norm(
array([
robot.device.state.value[28] -
robot.mTasks['posture'].ref[28],
robot.device.state.value[35] -
robot.mTasks['posture'].ref[35]
])) < 0.002:
robot.device.viewer.updateElementConfig('TwoHandTool',
(0., 0.5, 0., 0., 0., 0.))
goToHalfSitting(robot, solver, 5)
state = 23
print "time = " + str(robot.device.control.time)
# wait 200*dt (1 second) after state 1
if state >= 1000 and state < 1200:
robot.device.state.recompute(robot.device.control.time)
if linalg.norm(
array([
robot.device.state.value[28] -
robot.mTasks['posture'].ref[28],
robot.device.state.value[35] -
robot.mTasks['posture'].ref[35]
])) < 0.002:
state += 1
if state == 1200:
state = 1
print "time = " + str(robot.device.control.time)
# wait 200*dt (1 second) after state 8
if state >= 21000 and state < 21200:
robot.device.state.recompute(robot.device.control.time)
if linalg.norm(
array([
robot.device.state.value[28] -
robot.mTasks['posture'].ref[28],
robot.device.state.value[35] -
robot.mTasks['posture'].ref[35]
])) < 0.002:
state += 1
if state == 21200:
state = 22
print "time = " + str(robot.device.control.time)
(0., 0.0, 0., 0., 0., 0.))
P72RPY = (0.75, -0.45, 0.85, 0., 0., 1.57)
robot.device.viewer.updateElementConfig('P72', P72RPY)
P72 = RPYToMatrix(P72RPY)
#-----------------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------
#-----------------------------------------------------------------------------
p72togoal = Multiply_of_matrixHomo("p72togoal")
p72togoal.sin1.value = matrixToTuple(P72)
p72togoal.sin2.value = matrixToTuple(p72tohole[0])
goal = p72togoal.sout
goal.recompute(0)
robot.device.viewer.updateElementConfig(
'goal1', vectorToTuple(array(matrixToRPY(goal.value))))
get_2ht(robot, solver, tool, gainMax, gainMin)
state = 0
"""
# Motion recording
zmp_out = open("/tmp/data.zmp","w")
hip_out = open("/tmp/data.hip","w")
pos_out = open("/tmp/data.pos","w")
"""
taskCom.gain.setConstant(0.1)
taskPosture.ref = pose
# Set the target for RH and LH task. Selec is the activation flag (say control only
# the XYZ translation), and gain is the adaptive gain (10 at the target, 0.1
# far from it, with slope st. at 0.01m from the target, 90% of the max gain
# value is reached
displacementMatrix = eye(4)
displacementMatrix[0:3, 3] = array([0.1, 0., 0.])
taskRH.feature.position.recompute(0)
taskLH.feature.position.recompute(0)
targetRH = vectorToTuple(
array(
matrixToRPY(
dot(displacementMatrix, array(taskRH.feature.position.value)))))
gotoNd(taskRH, targetRH, "111111", (50, 1, 0.01, 0.9))
RHPos = taskRH.feature.position.value
LHPos = taskLH.feature.position.value
#3RHPos = vectorToTuple(array(taskRH.feature.position.value)[0:3,3])
#LHPos = vectorToTuple(array(taskLH.feature.position.value)[0:3,3])
gotoNdRel(taskRel, RHPos, LHPos, '110111', (50, 1, 0.01, 0.9))
taskRel.feature.errordot.value = (0, 0, 0, 0, 0) # not to forget!!
############################################################
# Production of variable reference vector
HToR = HomoToRotation("HToR")
plug(taskLH.feature.position, HToR.sin)
p72tohole = getData(0.)
tool = (0.4, -0.1, 0.8, 0., 0., pi / 2)
P72RPY = (0.75, -0.45, 0.85, 0., 0., 1.57)
P72 = RPYToMatrix(P72RPY)
robot.device.viewer.updateElementConfig('TwoHandTool',
(0., 0., 0., 0., 0., 0.))
robot.device.viewer.updateElementConfig('P72', P72RPY)
robot.device.viewer.updateElementConfig(
'goal1', vectorToTuple(array(matrixToRPY(dot(P72, p72tohole[0])))))
#-----------------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------
#-----------------------------------------------------------------------------
get_2ht(robot, solver, tool, gainMax, gainMin)
state = 0
"""
# Motion recording
zmp_out = open("/tmp/data.zmp","w")
hip_out = open("/tmp/data.hip","w")
pos_out = open("/tmp/data.pos","w")
"""
def displayFov(recompute=False):
if recompute: taskGaze.proj.transfo.recompute(robot.state.time)
robot.viewer.updateElementConfig('fov',matrixToRPY(taskGaze.proj.transfo.value))