def createReferenceVector(robot):
P72 = plugObjectSim(robot)
# the out signal is always the actual goal
HOLEINWORLD = Multiply_of_matrixHomo("HOLEINWORLD")
plug(P72, HOLEINWORLD.sin1)
return (HOLEINWORLD.sin2, HOLEINWORLD.sout)
def plugObject(robot):
robot.cameraFrameName = 'cameraBottomLeft'
# Change of reference orientation from Visp to dynamic_graph
visp2dg = ((0., 0., 1., 0.), (-1., 0., 0., 0.), (0., -1., 0., 0.),
(0., 0., 0., 1.))
# Multiplies the position of the object in the camera reference for the camera orientation of Visp
# The out signal is the object position in a reference located on the camera, but oriented as used in DG
OBJECTINCAMERA_DG = Multiply_of_matrixHomo("OBJECTINCAMERA_DG")
OBJECTINCAMERA_DG.sin1.value = visp2dg
plug(robot.rosExport.objectInCamera, OBJECTINCAMERA_DG.sin2)
# The out signal is objectInWorld
OBJECTINWORLD = Multiply_of_matrixHomo("OBJECTINWORLD")
plug(OBJECTINCAMERA_DG.sout, OBJECTINWORLD.sin2)
plug(robot.frames[robot.cameraFrameName].position, OBJECTINWORLD.sin1)
return OBJECTINWORLD.sout
def plugObjectFix(robot):
P72RPY = (0.75, -0.45, 0.85, 0., 0., 1.57)
oIw = RPYToMatrix(P72RPY)
robot.cameraFrameName = 'cameraBottomLeft'
inv = Inverse_of_matrixHomo("inv")
plug(robot.frames[robot.cameraFrameName].position, inv.sin)
mult = Multiply_of_matrixHomo("mult")
plug(inv.sout, mult.sin1)
mult.sin2.value = oIw
# The out signal is objectInWorld with simulated movement of the robot
OBJECTINWORLD = Multiply_of_matrixHomo("OBJECTINWORLD")
plug(mult.sout, OBJECTINWORLD.sin2)
plug(robot.frames[robot.cameraFrameName].position, OBJECTINWORLD.sin1)
return OBJECTINWORLD.sout
def plugObjectSim(robot):
objectInWorld = plugObject(robot)
objectInWorld.recompute(robot.device.control.time)
oIw = objectInWorld.value
#since the real robot is not moving we have to compensate the changing of the camera position
robot.cameraFrameName = 'cameraBottomLeft'
inv = Inverse_of_matrixHomo("inv")
plug(robot.frames[robot.cameraFrameName].position, inv.sin)
mult = Multiply_of_matrixHomo("mult")
plug(inv.sout, mult.sin1)
mult.sin2.value = oIw
# The out signal is objectInWorld with simulated movement of the robot
OBJECTINWORLD = Multiply_of_matrixHomo("OBJECTINWORLD")
plug(mult.sout, OBJECTINWORLD.sin2)
plug(robot.frames[robot.cameraFrameName].position, OBJECTINWORLD.sin1)
return OBJECTINWORLD.sout
def createReference(robot):
p72 = plugObject(robot)
# The out signal is the goal
HOLEINWORLD = Multiply_of_matrixHomo("HOLEINWORLD")
plug(p72, HOLEINWORLD.sin1)
security_distance = 0.7
orientation = array([[0., 1., 0.], [0., 0., -1], [-1., 0., 0.]])
p72tohole9 = eye(4)
p72tohole9[0:3, 0:3] = orientation
p72tohole9[0:3, 3] = array([0.1, 0.2 + security_distance, -0.25])
sigIN = HOLEINWORLD.sin2
sigIN.value = matrixToTuple(p72tohole9)
sigOUT = HOLEINWORLD.sout
return sigOUT
def initZMPRef(robot):
# --- Selector of Com Ref: when robot.pg is stopped, pg.inprocess becomes 0
robot.comSelector = Selector('comSelector',
['vector', 'ref', robot.com, robot.pg.comref])
plug(robot.pg.inprocess, robot.comSelector.selec)
selecSupportFoot = Selector(
'selecSupportFoot',
['matrixHomo', 'pg_H_sf', robot.pg.rightfootref, robot.pg.leftfootref],
['matrixHomo', 'wa_H_sf', robot.geom.rf2, robot.geom.lf2])
robot.addTrace(robot.pg.name, 'rightfootref')
robot.addTrace(robot.pg.name, 'leftfootref')
robot.addTrace(robot.pg.name, 'comref')
robot.addTrace(robot.pg.name, 'zmpref')
robot.addTrace(robot.pg.name, 'inprocess')
robot.addTrace(robot.device.name, 'forceLLEG')
robot.addTrace(robot.device.name, 'forceRLEG')
plug(robot.pg.SupportFoot, selecSupportFoot.selec)
sf_H_wa = Inverse_of_matrixHomo('sf_H_wa')
plug(selecSupportFoot.wa_H_sf, sf_H_wa.sin)
pg_H_wa = Multiply_of_matrixHomo('pg_H_wa')
plug(selecSupportFoot.pg_H_sf, pg_H_wa.sin1)
plug(sf_H_wa.sout, pg_H_wa.sin2)
# --- Compute the ZMP ref in the Waist reference frame.
wa_H_pg = Inverse_of_matrixHomo('wa_H_pg')
plug(pg_H_wa.sout, wa_H_pg.sin)
wa_zmp = Multiply_matrixHomo_vector('wa_zmp')
plug(wa_H_pg.sout, wa_zmp.sin1)
plug(robot.pg.zmpref, wa_zmp.sin2)
# Connect the ZMPref to OpenHRP in the waist reference frame.
robot.pg.parseCmd(':SetZMPFrame world')
plug(robot.pg.zmpref, robot.device.zmp)
robot.addTrace(robot.device.name, 'zmp')
robot.addTrace(pg_H_wa.name, 'sout')
p72tohole = getData(0.)
tool = (0.4, -0.1, 0.8, 0., 0., pi / 2)
robot.device.viewer.updateElementConfig('TwoHandTool',
(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")