def gotoq(self, gain=None, qdes=None, **kwargs):
act = list()
if qdes is not None:
if isinstance(qdes, tuple):
qdes = matrix(qdes).T
else:
if MetaTaskPosture.nbDof is None:
MetaTaskPosture.nbDof = len(self.feature.errorIN.value)
qdes = zeros((MetaTaskPosture.nbDof, 1))
act = [
False,
] * MetaTaskPosture.nbDof
for limbName, jointValues in kwargs.items():
limbRange = self.postureRange[limbName]
for i in limbRange:
act[i] = True
if jointValues != []:
if isinstance(jointValues, matrix):
qdes[limbRange, 0] = vectorToTuple(jointValues)
else:
qdes[limbRange, 0] = jointValues
self.ref = vectorToTuple(qdes)
if len(act) > 0:
self.feature.selec.value = Flags(act)
setGain(self.gain, gain)
def initTaskBalance(self):
# --- BALANCE ---
self.taskChest.feature.frame('desired')
self.taskChest.feature.selec.value = '011000'
ljl = matrix(self.robot.dynamic.lowerJl.value).T
ujl = matrix(self.robot.dynamic.upperJl.value).T
ljl[9] = 0.65
ljl[15] = 0.65
self.taskLim.referenceInf.value = vectorToTuple(ljl)
self.taskLim.referenceSup.value = vectorToTuple(ujl)
def gotoq(self, gain=None, **kwargs):
act = list()
if MetaTaskDynPosture.nbDof == None:
MetaTaskDynPosture.nbDof = len(self.feature.errorIN.value)
qdes = zeros((MetaTaskDynPosture.nbDof, 1))
for n, v in kwargs.items():
r = self.postureRange[n]
act += r
if isinstance(v, matrix): qdes[r, 0] = vectorToTuple(v)
if isinstance(v, ndarray): qdes[r, 0] = vectorToTuple(v)
else: qdes[r, 0] = v
self.ref = vectorToTuple(qdes)
self.feature.selec.value = toFlags(act)
setGain(self.gain, gain)
def gotoq(self,gain=None,**kwargs):
act=list()
if MetaTaskDynPosture.nbDof==None:
MetaTaskDynPosture.nbDof = len(self.feature.errorIN.value)
qdes = zeros((MetaTaskDynPosture.nbDof,1))
for n,v in kwargs.items():
r = self.postureRange[n]
act += r
if isinstance(v,matrix): qdes[r,0] = vectorToTuple(v)
if isinstance(v,ndarray): qdes[r,0] = vectorToTuple(v)
else: qdes[r,0] = v
self.ref = vectorToTuple(qdes)
self.feature.selec.value = toFlags(act)
setGain(self.gain,gain)
def updateComProj(com=None):
if com==None:
[x,y,z]=com=dyn.com.value
else:
[x,y,z]=com
p=array(comproj)+[0,y,-x]
robot.viewer.updateElementConfig('zmp2',vectorToTuple(p)+(0,0,0 ))
def screw_2ht(robot, solver, tool, target, goal, gainMax, gainMin):
#goal = array([0.5,-0.3,1.1,0.,1.57,0.])
if 'rel' not in robot.mTasks: createRelativeTask(robot)
if 'screw' not in robot.mTasks:
createScrewTask(robot, tool)
createVecMult(robot)
if not hasattr(robot, 'm2pos'): createM2Pos(robot)
# Task Relative
gotoNdRel(robot.mTasks['rel'],
array(robot.mTasks['rh'].feature.position.value),
array(robot.mTasks['lh'].feature.position.value), '110111',
gainMax * 2)
robot.mTasks['rel'].feature.errordot.value = (0, 0, 0, 0, 0
) # not to forget!!
# Aim setting
if isinstance(goal, ndarray):
if len(goal) == 6:
refToGoalMatrix = RPYToMatrix(goal)
else:
refToGoalMatrix = goal
robot.mTasks['screw'].ref = matrixToTuple(refToGoalMatrix)
robot.mTasks['screw'].featureVec.positionRef.value = dot(
refToGoalMatrix[0:3, 0:3], array([0., 0., 1.]))
else: #goal is a signal
plug(goal, robot.mTasks['screw'].featureDes.position)
plug(goal, robot.selec.sin)
robot.mult.sin2.value = (0., 0., 1.)
plug(robot.mult.sout, robot.mTasks['screw'].featureVec.positionRef)
if isinstance(target, ndarray):
if len(target) == 6:
refToTargetMatrix = RPYToMatrix(target)
else:
refToTargetMatrix = target
robot.mTasks['gaze'].proj.point3D.value = vectorToTuple(target[0:3, 3])
else: #target is a signal
plug(target, robot.m2pos.sin)
plug(robot.m2pos.sout, robot.mTasks['gaze'].proj.point3D)
robot.mTasks['gaze'].gain.setByPoint(gainMax * 2, gainMin * 2, 0.01, 0.9)
robot.mTasks['screw'].gain.setByPoint(gainMax, gainMin, 0.01, 0.9)
tasks = array([
robot.mTasks['rel'].task, robot.mTasks['gaze'].task,
robot.mTasks['screw'].task
])
# sot charging
if not (('taskrel' in solver.toList()) and
('taskgaze' in solver.toList()) and
('taskscrew' in solver.toList())):
removeUndesiredTasks(solver)
for i in range(len(tasks)):
solver.push(tasks[i])
def __call__(self,t):
self.vball += self.dt*array(self.gravity)
self.pball += self.dt*self.vball
if self.pball[2]<self.z0:
self.vball[2] = self.elasticZ * abs(self.vball[2])
self.pball[2] = self.z0
self.vball[0:2] = (nprand.rand(2)-0.5)*self.elasticXY
return vectorToTuple(self.pball)
def get_2ht(robot, solver, TwoHandTool, gainMax, gainMin):
#TwoHandTool = (0.4,-0.1,0.9,0.,0.,pi/2)
#TwoHandTool = (xd,yd,zd,roll,pitch,yaw)
# Homogeneous Matrix of the TwoHandTool. Normally given from the camera
refToTwoHandToolMatrix = RPYToMatrix(TwoHandTool)
# Homogeneous Matrixes
refToTriggerMatrix = dot(refToTwoHandToolMatrix,
TwoHandToolToTriggerMatrix)
refToSupportMatrix = dot(refToTwoHandToolMatrix,
TwoHandToolToSupportMatrix)
# ---- TASKS DEFINITION -------------------------------------------------------------------
# Set the targets. Selec is the activation flag (say control only
# the XYZ translation), and gain is the adaptive gain (<arg1> at the target, <arg2>
# far from it, with slope st. at <arg3>m from the target, <arg4>% of the max gain
# value is reached
target = vectorToTuple(refToSupportMatrix[0:3, 3])
gotoNd(robot.mTasks['rh'], target, "000111", (gainMax, gainMin, 0.01, 0.9))
target = vectorToTuple(refToTriggerMatrix[0:3, 3])
gotoNd(robot.mTasks['lh'], target, "000111", (gainMax, gainMin, 0.01, 0.9))
# Orientation RF and LF - Needed featureVector3 to get desired behaviour
if not hasattr(robot.mTasks['rh'], 'featureVec'):
createFeatureVec(robot, 'rh', array([1., 0., 0.]))
if not hasattr(robot.mTasks['lh'], 'featureVec'):
createFeatureVec(robot, 'lh', array([1., 0., 0.]))
robot.mTasks['rh'].featureVec.positionRef.value = dot(
refToTwoHandToolMatrix[0:3, 0:3], array([1., 0., 0.]))
robot.mTasks['lh'].featureVec.positionRef.value = dot(
refToTwoHandToolMatrix[0:3, 0:3], array([-1., 0., 0.]))
tasks = array([robot.mTasks['rh'].task, robot.mTasks['lh'].task])
# sot loading
solver.sot.damping.value = 0.001
removeUndesiredTasks(solver)
for i in range(len(tasks)):
solver.push(tasks[i])
def __call__(self, t):
self.vball += self.dt * array(self.gravity)
self.pball += self.dt * self.vball
if self.pball[2] < self.z0:
self.vball[2] = self.elasticZ * abs(self.vball[2])
self.pball[2] = self.z0
self.vball[0:2] = (nprand.rand(2) - 0.5) * self.elasticXY
return vectorToTuple(self.pball)
def record(self):
i = self.dynEnt.position.time
if i % self.freq == 0:
self.q.append(self.dynEnt.position.value)
self.qdot.append(self.dynEnt.velocity.value)
if self.withZmp:
waMwo = matrix(self.dynEnt.waist.value).I
wo_z = matrix(self.zmpSig.value + (1, )).T
self.zmp.append(list(vectorToTuple(waMwo * wo_z)))
def record(self):
i=self.dynEnt.position.time
if i%self.freq == 0:
self.q.append(self.dynEnt.position.value)
self.qdot.append(self.dynEnt.velocity.value)
if self.withZmp:
waMwo = matrix(self.dynEnt.waist.value).I
wo_z=matrix(self.zmpSig.value+(1,)).T
self.zmp.append(list(vectorToTuple(waMwo*wo_z)))
def trackTheBall(ball=None):
if ball==None:
t=robot.state.time/200.0
#ball = h * cos(array(w)*t) + ball0
ball = ballTraj(t)
if isinstance(ball,ndarray): ball=vectorToTuple(ball)
gotoNd(taskRH,ball)
gotoNd(taskGaze,ball)
robot.viewer.updateElementConfig('zmp',ball+(0,0,0))
def trackTheBall(ball=None):
if ball == None:
t = robot.state.time / 200.0
#ball = h * cos(array(w)*t) + ball0
ball = ballTraj(t)
if isinstance(ball, ndarray): ball = vectorToTuple(ball)
gotoNd(taskRH, ball)
gotoNd(taskGaze, ball)
robot.viewer.updateElementConfig('zmp', ball + (0, 0, 0))
def moveDisplay(self):
tau = 1.0 if self.duration<=0 else float(self.t) / self.duration
xyz = tau * array(self.ball) + (1-tau) * array(self.prec)
robot.device.viewer.updateElementConfig('zmp',vectorToTuple(xyz)+(0,0,0))
self.t += 1
if self.t>self.duration and self.duration>0:
attime.stop(self.f)
self.xyz= xyz
def moveHead(robot, solver, angle):
# Pose definition
pose = array(robot.halfSitting)
pose[20] = angle
# New taskPosture creation
robot.mTasks['posture'].ref = vectorToTuple(pose)
robot.mTasks['posture'].gain.setConstant(10)
def moveDisplay(self):
tau = 1.0 if self.duration <= 0 else float(self.t) / self.duration
xyz = tau * array(self.ball) + (1 - tau) * array(self.prec)
robot.viewer.updateElementConfig('zmp', vectorToTuple(xyz) + (0, 0, 0))
self.t += 1
if self.t > self.duration and self.duration > 0:
attime.stop(self.f)
self.xyz = xyz
def openGrippers(robot, solver):
# Pose definition
pose = array(robot.halfSitting)
pose[28] = 0.74
pose[35] = 0.74
# New taskPosture creation
robot.mTasks['posture'].ref = vectorToTuple(pose)
robot.mTasks['posture'].gain.setConstant(10)
def gotoq(self,gain=None,qdes=None,**kwargs):
act=list()
if qdes!=None:
if isinstance(qdes,tuple): qdes=matrix(qdes).T
else:
if MetaTaskPosture.nbDof==None:
MetaTaskPosture.nbDof = len(self.feature.errorIN.value)
qdes = zeros((MetaTaskPosture.nbDof,1))
for limbName,jointValues in kwargs.items():
limbRange = self.postureRange[limbName]
act += limbRange
if jointValues!=[]:
if isinstance(jointValues,matrix):
qdes[limbRange,0] = vectorToTuple(jointValues)
else: qdes[limbRange,0] = jointValues
self.ref = vectorToTuple(qdes)
if(len(act)>0): self.feature.selec.value = toFlags(act)
setGain(self.gain,gain)
def goToHalfSitting(robot, solver, gain):
# Remove Other Tasks
removeUndesiredTasks(solver)
# Pose definition
pose = array(robot.halfSitting)
# New taskPosture creation
robot.mTasks['posture'].ref = vectorToTuple(pose)
robot.mTasks['posture'].gain.setConstant(gain)
def goto6d(task, position, gain=None):
M = eye(4)
if isinstance(position, matrix): position = vectorToTuple(position)
if (len(position) == 3): M[0:3, 3] = position
else:
#print "Position 6D with rotation ... todo" # done?
M = array(rpy2tr(*position[3:7]))
M[0:3, 3] = position[0:3]
task.feature.selec.value = "111111"
setGain(task.gain, gain)
task.featureDes.position.value = matrixToTuple(M)
task.task.resetJacobianDerivative()
def goto6d(task,position,gain=None):
M=eye(4)
if isinstance(position,matrix): position = vectorToTuple(position)
if( len(position)==3 ): M[0:3,3] = position
else:
#print "Position 6D with rotation ... todo" # done?
M = array( rpy2tr(*position[3:7]) )
M[0:3,3] = position[0:3]
task.feature.selec.value = "111111"
setGain(task.gain,gain)
task.featureDes.position.value = matrixToTuple(M)
task.task.resetJacobianDerivative()
def gotoNd(task, position, selec, gain=None, resetJacobian=True):
M = eye(4)
if isinstance(position, matrix): position = vectorToTuple(position)
if (len(position) == 3): M[0:3, 3] = position
else:
#print "Position 6D with rotation ... todo" # done?
M = array(rpy2tr(*position[3:7]))
M[0:3, 3] = position[0:3]
if isinstance(selec, str): task.feature.selec.value = selec
else: task.feature.selec.value = toFlags(selec)
task.featureDes.position.value = matrixToTuple(M)
if resetJacobian: task.task.resetJacobianDerivative()
setGain(task.gain, gain)
def gotoNd(task,position,selec,gain=None,resetJacobian=True):
M=eye(4)
if isinstance(position,matrix): position = vectorToTuple(position)
if( len(position)==3 ): M[0:3,3] = position
else:
#print "Position 6D with rotation ... todo" # done?
M = array( rpy2tr(*position[3:7]) )
M[0:3,3] = position[0:3]
if isinstance(selec,str): task.feature.selec.value = selec
else: task.feature.selec.value = toFlags(selec)
task.featureDes.position.value = matrixToTuple(M)
if resetJacobian: task.task.resetJacobianDerivative()
setGain(task.gain,gain)
def setTaskLim(taskJL,robot):
"""
Sets the parameters for the 'joint-limits'
"""
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
plug(robot.dynamic.position,taskJL.position)
taskJL.controlGain.value = 10
refInf = array(robot.dynamic.lowerJl.value)
# Avoid knee singularity
refInf[9] = 0.7
refInf[15] = 0.7
taskJL.referenceInf.value = vectorToTuple(refInf)
taskJL.referenceSup.value = robot.dynamic.upperJl.value
taskJL.dt.value = robot.timeStep
taskJL.selec.value = toFlags(range(6,22)+range(22,28)+range(29,35))
def goto6dPP(task, position, velocity, duration, current):
if isinstance(position, matrix): position = vectorToTuple(position)
if (len(position) == 3):
''' If only position specification '''
M = eye(4)
M[0:3, 3] = position
else:
''' If there is position and orientation '''
M = array(rpy2tr(*position[3:7]))
M[0:3, 3] = position[0:3]
task.feature.selec.value = "111111"
task.task.controlSelec.value = "111111"
task.featureDes.position.value = matrixToTuple(M)
task.task.duration.value = duration
task.task.initialTime.value = current
task.task.velocityDes.value = velocity
task.task.resetJacobianDerivative()
def goto6dPP(task, position, velocity, duration, current):
if isinstance(position,matrix): position = vectorToTuple(position)
if( len(position)==3 ):
''' If only position specification '''
M=eye(4)
M[0:3,3] = position
else:
''' If there is position and orientation '''
M = array( rpy2tr(*position[3:7]) )
M[0:3,3] = position[0:3]
task.feature.selec.value = "111111"
task.task.controlSelec.value = "111111"
task.featureDes.position.value = matrixToTuple(M)
task.task.duration.value = duration
task.task.initialTime.value = current
task.task.velocityDes.value = velocity
task.task.resetJacobianDerivative()
def gotoNdPP(task, position, velocity, selec, duration, current):
if isinstance(position,matrix): position = vectorToTuple(position)
if( len(position)==3 ):
M=eye(4)
M[0:3,3] = position
else:
M = array( rpy2tr(*position[3:7]) )
M[0:3,3] = position[0:3]
if isinstance(selec,str):
task.feature.selec.value = selec
task.task.controlSelec.value = selec
else:
task.feature.selec.value = toFlags(selec)
task.task.controlSelec.value = toFlags(selec)
task.featureDes.position.value = matrixToTuple(M)
task.task.duration.value = duration
task.task.initialTime.value = current
task.task.velocityDes.value = velocity
task.task.resetJacobianDerivative()
def gotoNdPP(task, position, velocity, selec, duration, current):
if isinstance(position, matrix): position = vectorToTuple(position)
if (len(position) == 3):
M = eye(4)
M[0:3, 3] = position
else:
M = array(rpy2tr(*position[3:7]))
M[0:3, 3] = position[0:3]
if isinstance(selec, str):
task.feature.selec.value = selec
task.task.controlSelec.value = selec
else:
task.feature.selec.value = toFlags(selec)
task.task.controlSelec.value = toFlags(selec)
task.featureDes.position.value = matrixToTuple(M)
task.task.duration.value = duration
task.task.initialTime.value = current
task.task.velocityDes.value = velocity
task.task.resetJacobianDerivative()
def setTaskLim(taskLim,robot):
"""
Sets the parameters for teh 'task-limits'
"""
# Angular position and velocity limits
plug(robot.dynamic.position,taskLim.position)
plug(robot.dynamic.velocity,taskLim.velocity)
taskLim.dt.value = robot.timeStep
robot.dynamic.upperJl.recompute(0)
robot.dynamic.lowerJl.recompute(0)
refInf = array(robot.dynamic.lowerJl.value)
# Avoid knee singularity
refInf[9] = 0.7
refInf[15] = 0.7
taskLim.referencePosInf.value = vectorToTuple(refInf)
taskLim.referencePosSup.value = robot.dynamic.upperJl.value
#dqup = (0, 0, 0, 0, 0, 0, 200, 220, 250, 230, 290, 520, 200, 220, 250, 230, 290, 520, 250, 140, 390, 390, 240, 140, 240, 130, 270, 180, 330, 240, 140, 240, 130, 270, 180, 330)
dqup = (1000,)*robot.dimension ########################
taskLim.referenceVelInf.value = tuple([-val*3.14/180 for val in dqup])
taskLim.referenceVelSup.value = tuple([ val*3.14/180 for val in dqup])
taskLim.controlGain.value = 0.3
def ballInHand():
#rh = array(dyn.rh.value)[0:3,3]
robot.after.addSignal(taskRH.feature.name + '.position')
rh = array(taskRH.feature.position.value)[0:3, 3]
robot.viewer.updateElementConfig('zmp', vectorToTuple(rh) + (0, 0, 0))
def i():
xyz = ball.xyz
xyz[0] += 0.1
ball.move(vectorToTuple(xyz), 30)
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
robot.dynamic.lowerJl.recompute(0)
robot.dynamic.upperJl.recompute(0)
llimit = matrix(robot.dynamic.lowerJl.value)
ulimit = matrix(robot.dynamic.upperJl.value)
dlimit = ulimit-llimit
mlimit = (ulimit+llimit)/2
llimit[6:18] = mlimit[6:12] - dlimit[6:12]*0.48
robot.dynamic.upperJl.value = vectorToTuple(ulimit)
plug(robot.device.state, robot.dynamic.position)
plug(robot.device.velocity,robot.dynamic.velocity)
robot.dynamic.acceleration.value = robot.dimension*(0.,)
# set the free flyer free.
robot.dynamic.ffposition.unplug()
robot.dynamic.ffvelocity.unplug()
robot.dynamic.ffacceleration.unplug()
robot.dynamic.setProperty('ComputeBackwardDynamics','true')
robot.dynamic.setProperty('ComputeAccelerationCoM','true')
#-----------------------------------------------------------------------------
# --- OPERATIONAL TASKS (For HRP2-14)---------------------------------------------
def supervision():
global state, tool, robot, solver, pos_err_des, gainMax, gainMin, goal
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(goal[0])
screw_2ht(robot, solver, tool, P72, goal[0], 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(goal) + 2:
print "Goal: " + str(goal[state - 2])
screw_2ht(robot, solver, tool, P72, goal[state - 2], gainMax,
gainMin)
robot.device.viewer.updateElementConfig(
'goal1', vectorToTuple(goal[state - 2]))
if state == len(goal) + 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, 1)
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)
goal5 = goal3 + 0.4 * (goal8 - goal3)
goal6 = goal3 + 0.6 * (goal8 - goal3)
goal7 = goal3 + 0.8 * (goal8 - goal3)
goal11 = goal10 + 0.2 * (goal1 - goal10)
goal12 = goal10 + 0.4 * (goal1 - goal10)
goal13 = goal10 + 0.6 * (goal1 - goal10)
goal14 = goal10 + 0.8 * (goal1 - goal10)
#goal = array([goal1,goal2,goal3,goal4,goal5,goal6,goal7,goal8,goal9,goal10])
goal = array([
goal9, goal10, goal11, goal12, goal13, goal14, goal1, goal2, goal3, goal4,
goal5, goal6, goal7, goal8
])
robot.device.viewer.updateElementConfig('goal1', vectorToTuple(goal[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")
"""
(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")
"""
def i():
xyz=ball.xyz
xyz[0] += 0.1
ball.move(vectorToTuple(xyz),30)
def ballInHand():
#rh = array(dyn.rh.value)[0:3,3]
robot.after.addSignal(taskRH.feature.name + '.position')
rh = array(taskRH.feature.position.value)[0:3,3]
robot.viewer.updateElementConfig('zmp',vectorToTuple(rh)+(0,0,0))
def test_vector_to_tuple(self):
vec = (1, 2, 3, 4)
self.assertEqual(vec, mod.vectorToTuple(np.matrix(vec)))
self.assertEqual(vec, mod.vectorToTuple(np.array(vec)))
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")
"""
xmlDir = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/' + specificitiesName[robotName]
jointRankPath = xmlDir + '/' + jointRankName[robotName]
dyn = Dynamic("dyn")
dyn.setFiles(modelDir, modelName[robotName], specificitiesPath, jointRankPath)
dyn.parse()
dyn.lowerJl.recompute(0)
dyn.upperJl.recompute(0)
llimit = matrix(dyn.lowerJl.value)
ulimit = matrix(dyn.upperJl.value)
dlimit = ulimit - llimit
mlimit = (ulimit + llimit) / 2
llimit[6:18] = mlimit[6:12] - dlimit[6:12] * 0.48
dyn.upperJl.value = vectorToTuple(ulimit)
dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]
plug(robot.state, dyn.position)
plug(robot.velocity, dyn.velocity)
dyn.acceleration.value = robotDim * (0., )
dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()
dyn.setProperty('ComputeBackwardDynamics', 'true')
dyn.setProperty('ComputeAccelerationCoM', 'true')
def supervision():
global state, tool, robot, solver, pos_err_des, gain, yMin, yMax, zMin, zMax, getPose, goal
if state == 'init':
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:
state = 'tryF'
goal = random_goal(yMin, yMax, zMin, zMax)
getPose = robot.device.state.value
print "Goal: " + str(goal)
robot.device.viewer.updateElementConfig('goal1',
vectorToTuple(goal))
screw_2ht(robot, solver, tool, goal, gain)
if state == 'tryF':
robot.mTasks['screw'].feature.error.recompute(
robot.device.control.time)
controlNorm = linalg.norm(array(robot.device.control.value))
if controlNorm > 100:
print "\nControl Norm: " + str(controlNorm)
if linalg.norm(array(
robot.mTasks['screw'].feature.error.value)[0:3]) < pos_err_des:
print "\nSUCCESS \n"
fRes.append((goal[1], goal[2], True))
f_out.write(str((goal[1], goal[2], True)) + '\n')
state = 'success'
#get_2ht(robot,solver,tool,gain)
robot.device.setVelocity((0, ) * 36)
robot.device.set(getPose)
elif controlNorm > 300:
print "\nFAILURE \n"
fRes.append((goal[1], goal[2], False))
f_out.write(str((goal[1], goal[2], False)) + '\n')
state = 'failure'
robot.device.setVelocity((0, ) * 36)
robot.device.set(getPose)
if state == 'success':
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:
state = 'tryF'
goal = random_goal(yMin, yMax, zMin, zMax)
print "Goal: " + str(goal)
robot.device.viewer.updateElementConfig('goal1',
vectorToTuple(goal))
screw_2ht(robot, solver, tool, goal, gain)
if state == 'failure':
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:
state = 'tryF'
goal = random_goal(yMin, yMax, zMin, zMax)
print "Goal: " + str(goal)
robot.device.viewer.updateElementConfig('goal1',
vectorToTuple(goal))
screw_2ht(robot, solver, tool, goal, gain)
