def setupTasks(self, opSpaceController):
robot = self.robot
feet = robotinfo.feet(robot)
# priority 1
# right foot task -- stay still relative to left foot
RFTask = opSpaceController.addTask(LinkTask(robot, feet['r'], "po",
feet['l']),
priority=1,
name="RF",
gains=(0.0, 0, 0))
# priority 2
# CoM task
self.comTask = opSpaceController.addTask(COMTask(robot),
priority=2,
gains=([-60.0, -30.0, -10.0],
[-0.4, -0.4,
-0.2], 0.0),
weight=[1.0, 1.0, 1.0])
#correct for com errors only using legs and hip
self.comTask.activeDofs = robotinfo.legsAndBase(robot)
#self.comTask.vcmdmax = 0.25
#approximation to ZMP criterion
#self.comTask.dvcmdmax = 100.0
#store local position of COM relative to feet
self.comLocalHomePos = {}
for k, v in feet.iteritems():
self.comLocalHomePos[v] = se3.apply(
se3.inv(robot.link(v).getTransform()),
self.comTask.getDesiredValue())
"""
def setupTasks(self,opSpaceController):
robot = self.robot
feet = robotinfo.feet(robot)
# priority 1
# right foot task -- stay still relative to left foot
RFTask = opSpaceController.addTask(LinkTask(robot, feet['r'], "po", feet['l']),
priority=1,
name="RF",
gains=(0.0,0,0))
# priority 2
# CoM task
self.comTask = opSpaceController.addTask(COMTask(robot),
priority=2,
gains=([-60.0,-30.0,-10.0], [-0.4,-0.4,-0.2], 0.0),
weight=[1.0,1.0,1.0])
#correct for com errors only using legs and hip
self.comTask.activeDofs = robotinfo.legsAndBase(robot)
#self.comTask.vcmdmax = 0.25
#approximation to ZMP criterion
#self.comTask.dvcmdmax = 100.0
#store local position of COM relative to feet
self.comLocalHomePos = {}
for k,v in feet.iteritems():
self.comLocalHomePos[v] = se3.apply(se3.inv(robot.link(v).getTransform()),self.comTask.getDesiredValue())
"""
def output_and_advance(self, **inputs):
lf = inputs['LF_ForceSensor']
rf = inputs['RF_ForceSensor']
#print lf,rf
out = {}
contactLinks = []
if self.lfscale * lf[2] > self.lfthresh:
out['lfcontact'] = 1
contactLinks.append(robotinfo.feet(self.robot)['l'])
else:
out['lfcontact'] = 0
if self.rfscale * rf[2] > self.rfthresh:
out['rfcontact'] = 1
contactLinks.append(robotinfo.feet(self.robot)['r'])
else:
out['rfcontact'] = 0
print "Estimated links in contact", contactLinks
out['contactLinks'] = contactLinks
return out
def output_and_advance(self,**inputs):
lf = inputs['LF_ForceSensor']
rf = inputs['RF_ForceSensor']
#print lf,rf
out = {}
contactLinks = []
if self.lfscale*lf[2] > self.lfthresh:
out['lfcontact']=1
contactLinks.append(robotinfo.feet(self.robot)['l'])
else:
out['lfcontact']=0
if self.rfscale*rf[2] > self.rfthresh:
out['rfcontact']=1
contactLinks.append(robotinfo.feet(self.robot)['r'])
else:
out['rfcontact']=0
print "Estimated links in contact",contactLinks
out['contactLinks'] = contactLinks
return out
def __init__(self,robot,jointStiffness=0,jointRecovery=1):
self.jointStiffness = jointStiffness
self.jointRecovery = jointRecovery
OpSpaceController.__init__(self,robot)
# set the motion model to assume that the feet are fixed in place
assert(robotinfo.freeBase(robot)==True)
feet = robotinfo.feet(robot)
assert len(feet)==2
self.setMotionModel(FreeBaseRobotMotionModel('velocity','velocity',robot,feet.values()))
def __init__(self, robot, jointStiffness=0, jointRecovery=1):
self.jointStiffness = jointStiffness
self.jointRecovery = jointRecovery
OpSpaceController.__init__(self, robot)
# set the motion model to assume that the feet are fixed in place
assert (robotinfo.freeBase(robot) == True)
feet = robotinfo.feet(robot)
assert len(feet) == 2
self.setMotionModel(
FreeBaseRobotMotionModel('velocity', 'velocity', robot,
feet.values()))
