def demo():
import numpy
init()
# set initial pose from sample/controller/SampleController/etc/Sample.pos
initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0.637045, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, -0.637045, 0, 0, 0]
hcf.seq_svc.setJointAngles(initial_pose, 2.5)
hcf.seq_svc.waitInterpolation()
# 1. force and moment are large because of link offsets
fm=numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_rhsensor")).data)
print "no-offset-removed force moment (rhsensor) ", fm, "=> ", fm > 1e-2
fm=numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data)
print "no-offset-removed force moment (lhsensor) ", fm, "=> ", fm > 1e-2
# 2. Set link offsets
# link_offset_centroid and link_offset_mass are identified value.
hcf.rmfo_svc.setForceMomentOffsetParam("rhsensor", OpenHRP.RemoveForceSensorLinkOffsetService.forcemomentOffsetParam(force_offset=[0,0,0], moment_offset=[0,0,0], link_offset_centroid=[0,0.0368,-0.076271], link_offset_mass=0.800011))
hcf.rmfo_svc.setForceMomentOffsetParam("lhsensor", OpenHRP.RemoveForceSensorLinkOffsetService.forcemomentOffsetParam(force_offset=[0,0,0], moment_offset=[0,0,0], link_offset_centroid=[0,-0.0368,-0.076271], link_offset_mass=0.800011))
ret = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")
if ret[0] and ret[1].link_offset_mass == 0.800011:
print "getForceMomentOffsetParam(\"rhsensor\") => OK"
ret = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")
if ret[0] and ret[1].link_offset_mass == 0.800011:
print "getForceMomentOffsetParam(\"lhsensor\") => OK"
# 3. force and moment are reduced
fm=numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_rhsensor")).data)
print "no-offset-removed force moment (rhsensor) ", fm, "=> ", fm < 1e-2
fm=numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data)
print "no-offset-removed force moment (lhsensor) ", fm, "=> ", fm < 1e-2
# 4. dump and load parameter file
ret=hcf.rmfo_svc.dumpForceMomentOffsetParams("/tmp/test-rmfo-offsets.dat")
if ret:
print "dumpForceMomentOffsetParams => OK"
ret=hcf.rmfo_svc.loadForceMomentOffsetParams("/tmp/test-rmfo-offsets.dat")
if ret:
print "loadForceMomentOffsetParams => OK"
def demoDualarmPush (auto_detecion = True):
print >> sys.stderr, "3. Dual-arm push demo."
print >> sys.stderr, " Move to"
hcf.abc_svc.goPos(-0.45,0,0);
hcf.abc_svc.waitFootSteps();
hcf.abc_svc.goPos(0,0,(math.degrees(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.orientation.y-rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation.y)));
hcf.abc_svc.waitFootSteps();
print >> sys.stderr, " Reaching"
#hcf.abc_svc.goPos(0.25, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0);
hcf.abc_svc.goPos(0.1, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0);
hcf.abc_svc.waitFootSteps();
hcf.seq_svc.setJointAngles(dualarm_via_pose, 1.0)
hcf.waitInterpolation()
hcf.seq_svc.setJointAngles(dualarm_push_pose, 1.0)
hcf.waitInterpolation()
print >> sys.stderr, " Increase operational force"
if auto_detecion:
objectTurnaroundDetection(axis=[-1,0,0],max_ref_force=100, max_time=2)
else:
hcf.seq_svc.setWrenches([0]*6+[0]*6+[-40,0,0,0,0,0]*2, 2.0)
hcf.waitInterpolation()
print >> sys.stderr, " Push forward"
abcp=hcf.abc_svc.getAutoBalancerParam()[1]
abcp.is_hand_fix_mode = True
hcf.abc_svc.setAutoBalancerParam(abcp)
hcf.abc_svc.goPos(0.5,0,0)
hcf.abc_svc.waitFootSteps();
hcf.seq_svc.setWrenches([0]*24, 2.0)
hcf.waitInterpolation()
def checkWaist(var_doc):
bpos = rtm.readDataPort(hcf.sh.port("basePosOut")).data
brpy = rtm.readDataPort(hcf.sh.port("baseRpyOut")).data
ret = checkArrayEquality([bpos.x, bpos.y, bpos.z, brpy.r, brpy.p, brpy.y],
var_doc['waist'])
print " waist => ", ret
assert (ret is True)
def demoDualarmPush (auto_detecion = True):
print >> sys.stderr, "3. Dual-arm push demo."
print >> sys.stderr, " Move to"
hcf.abc_svc.goPos(-0.45,0,0);
hcf.abc_svc.waitFootSteps();
hcf.abc_svc.goPos(0,0,(math.degrees(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.orientation.y-rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation.y)));
hcf.abc_svc.waitFootSteps();
print >> sys.stderr, " Reaching"
#hcf.abc_svc.goPos(0.25, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0);
hcf.abc_svc.goPos(0.1, -1*(rtm.readDataPort(rtm.findRTC("PushBox(Robot)0").port("WAIST")).data.position.x-rtm.readDataPort(hcf.rh.port("WAIST")).data.position.x), 0);
hcf.abc_svc.waitFootSteps();
hcf.seq_svc.setJointAngles(dualarm_via_pose, 1.0)
hcf.waitInterpolation()
hcf.seq_svc.setJointAngles(dualarm_push_pose, 1.0)
hcf.waitInterpolation()
print >> sys.stderr, " Increase operational force"
if auto_detecion:
objectTurnaroundDetection(axis=[-1,0,0],max_ref_force=100, max_time=2)
else:
hcf.seq_svc.setWrenches([0]*6+[0]*6+[-40,0,0,0,0,0]*2, 2.0)
hcf.waitInterpolation()
print >> sys.stderr, " Push forward"
hcf.abc_svc.goPos(0.5,0,0)
hcf.abc_svc.waitFootSteps();
hcf.seq_svc.setWrenches([0]*24, 2.0)
hcf.waitInterpolation()
def demoSetForceMomentOffsetParam ():
print >> sys.stderr, "2. SetForceMomentOffsetParam"
print >> sys.stderr, " Force and moment are large because of link offsets"
for fs_name in ["rhsensor", "lhsensor"]:
fm = numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_"+fs_name)).data)
vret = fm > 5e-2
print >> sys.stderr, " no-offset-removed force moment (",fs_name,") ", fm, "=> ", vret
assert(vret)
print >> sys.stderr, " Set link offsets (link_offset_centroid and link_offset_mass are identified value)."
# Get param
r_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")[1]
r_fmop.link_offset_centroid = [0,0.0368,-0.076271]
r_fmop.link_offset_mass = 0.80011
l_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")[1]
l_fmop.link_offset_centroid = [0,-0.0368,-0.076271]
l_fmop.link_offset_mass = 0.80011
# Set param
hcf.rmfo_svc.setForceMomentOffsetParam("rhsensor", r_fmop)
hcf.rmfo_svc.setForceMomentOffsetParam("lhsensor", l_fmop)
# Check values
ret = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")
if ret[0] and ret[1].link_offset_mass == r_fmop.link_offset_mass and ret[1].link_offset_centroid == r_fmop.link_offset_centroid:
print >> sys.stderr, " getForceMomentOffsetParam('rhsensor') => OK"
assert((ret[0] and ret[1].link_offset_mass == r_fmop.link_offset_mass and ret[1].link_offset_centroid == r_fmop.link_offset_centroid))
ret = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")
if ret[0] and ret[1].link_offset_mass == l_fmop.link_offset_mass and ret[1].link_offset_centroid == l_fmop.link_offset_centroid:
print >> sys.stderr, " getForceMomentOffsetParam('lhsensor') => OK"
assert((ret[0] and ret[1].link_offset_mass == l_fmop.link_offset_mass and ret[1].link_offset_centroid == l_fmop.link_offset_centroid))
print >> sys.stderr, " Force and moment are reduced"
for fs_name in ["rhsensor", "lhsensor"]:
fm = numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_"+fs_name)).data)
vret = fm < 5e-2
print >> sys.stderr, " no-offset-removed force moment (",fs_name,") ", fm, "=> ", vret
assert(vret)
def demoStandingPosResetting():
print >> sys.stderr, "demoStandingPosResetting"
hcf.abc_svc.goPos(
0, 0,
math.degrees(
-1 * rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation.y))
hcf.abc_svc.waitFootSteps()
hcf.abc_svc.goPos(
0, -1 * rtm.readDataPort(hcf.rh.port("WAIST")).data.position.y, 0)
hcf.abc_svc.waitFootSteps()
def checkActualBaseAttitude(ref_rpy = None, thre=0.1): # degree
'''Check whether the robot falls down based on actual robot base-link attitude.
'''
act_rpy = rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation
if ref_rpy == None:
ref_rpy = rtm.readDataPort(hcf.abc.port("baseRpyOut")).data
ret = abs(math.degrees(act_rpy.r-ref_rpy.r)) < thre and abs(math.degrees(act_rpy.p-ref_rpy.p)) < thre
print >> sys.stderr, " ret = ", ret, ", actual base rpy = (", act_rpy, "), ", "reference base rpy = (", ref_rpy, ")"
assert (ret)
return ret
def checkActualBaseAttitude(ref_rpy=None, thre=0.1): # degree
'''Check whether the robot falls down based on actual robot base-link attitude.
'''
act_rpy = rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation
if ref_rpy == None:
ref_rpy = rtm.readDataPort(hcf.abc.port("baseRpyOut")).data
ret = abs(math.degrees(act_rpy.r - ref_rpy.r)) < thre and abs(
math.degrees(act_rpy.p - ref_rpy.p)) < thre
print >> sys.stderr, " ret = ", ret, ", actual base rpy = (", act_rpy, "), ", "reference base rpy = (", ref_rpy, ")"
assert (ret)
return ret
def checkJointAngles (var_doc):
if isinstance(var_doc, list):
p = var_doc
else:
p = var_doc['pos']
ret = checkArrayEquality(rtm.readDataPort(hcf.sh.port("qOut")).data, p)
print " pos => ", ret
def take_one_frame():
global md_port
vs_svc.take_one_frame()
dat = rtm.readDataPort(vs_port, classname='Img.TimedMultiCameraImageHolder')
print dat
print dat.data
if md_port != None:
rtm.writeDataPort(md_port, dat)
def checkWrenches(var_doc):
ret = checkArrayEquality(
reduce(
lambda x, y: x + y,
map(lambda fs: rtm.readDataPort(hcf.sh.port(fs + "Out")).data,
['lfsensor', 'rfsensor', 'lhsensor', 'rhsensor'])),
var_doc['wrenches'])
print " wrenches => ", ret
assert (ret is True)
def setAndCheckJointLimit (joint_name):
print >> sys.stderr, " ", joint_name
# ulimit check
link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0]
hcf.seq_svc.setJointAngle(joint_name, math.radians(1)+link_info.ulimit[0], 1)
hcf.waitInterpolation()
ret = rtm.readDataPort(hcf.el.port("q")).data[link_info.jointId] <= link_info.ulimit[0]
print >> sys.stderr, " ulimit = ", ret, "(elout=", rtm.readDataPort(hcf.el.port("q")).data[link_info.jointId], ", limit=", link_info.ulimit[0], ")"
assert(ret)
# llimit check
hcf.seq_svc.setJointAngle(joint_name, math.radians(-1)+link_info.llimit[0], 1)
hcf.waitInterpolation()
ret = rtm.readDataPort(hcf.el.port("q")).data[link_info.jointId] >= link_info.llimit[0]
print >> sys.stderr, " llimit = ", ret, "(elout=", rtm.readDataPort(hcf.el.port("q")).data[link_info.jointId], ", limit=", link_info.llimit[0], ")"
assert(ret)
# go to initial
hcf.seq_svc.setJointAngles(initial_pose, 1.0)
hcf.waitInterpolation()
def calcCOP():
cop_info = rtm.readDataPort(hcf.st.port("COPInfo")).data
lcopx = cop_info[1] / cop_info[2]
lcopy = cop_info[0] / cop_info[2]
rcopx = cop_info[1 + 3] / cop_info[2 + 3]
rcopy = cop_info[0 + 3] / cop_info[2 + 3]
return [
[lcopx, lcopx],
[rcopx, rcopy], # l cop, r cop
[(cop_info[1] + cop_info[1 + 3]) / (cop_info[2] + cop_info[2 + 3]),
(cop_info[0] + cop_info[0 + 3]) / (cop_info[2] + cop_info[2 + 3])]
] # total ZMP
def demoSetForceMomentOffsetParam():
print >> sys.stderr, "2. SetForceMomentOffsetParam"
print >> sys.stderr, " Force and moment are large because of link offsets"
for fs_name in ["rhsensor", "lhsensor"]:
fm = numpy.linalg.norm(
rtm.readDataPort(hcf.rmfo.port("off_" + fs_name)).data)
vret = fm > 5e-2
print >> sys.stderr, " no-offset-removed force moment (", fs_name, ") ", fm, "=> ", vret
assert (vret)
print >> sys.stderr, " Set link offsets (link_offset_centroid and link_offset_mass are identified value)."
# Get param
r_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")[1]
r_fmop.link_offset_centroid = [0, 0.0368, -0.076271]
r_fmop.link_offset_mass = 0.80011
l_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")[1]
l_fmop.link_offset_centroid = [0, -0.0368, -0.076271]
l_fmop.link_offset_mass = 0.80011
# Set param
hcf.rmfo_svc.setForceMomentOffsetParam("rhsensor", r_fmop)
hcf.rmfo_svc.setForceMomentOffsetParam("lhsensor", l_fmop)
# Check values
ret = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")
if ret[0] and ret[1].link_offset_mass == r_fmop.link_offset_mass and ret[
1].link_offset_centroid == r_fmop.link_offset_centroid:
print >> sys.stderr, " getForceMomentOffsetParam('rhsensor') => OK"
assert ((ret[0] and ret[1].link_offset_mass == r_fmop.link_offset_mass
and ret[1].link_offset_centroid == r_fmop.link_offset_centroid))
ret = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")
if ret[0] and ret[1].link_offset_mass == l_fmop.link_offset_mass and ret[
1].link_offset_centroid == l_fmop.link_offset_centroid:
print >> sys.stderr, " getForceMomentOffsetParam('lhsensor') => OK"
assert ((ret[0] and ret[1].link_offset_mass == l_fmop.link_offset_mass
and ret[1].link_offset_centroid == l_fmop.link_offset_centroid))
print >> sys.stderr, " Force and moment are reduced"
for fs_name in ["rhsensor", "lhsensor"]:
fm = numpy.linalg.norm(
rtm.readDataPort(hcf.rmfo.port("off_" + fs_name)).data)
vret = fm < 5e-2
print >> sys.stderr, " no-offset-removed force moment (", fs_name, ") ", fm, "=> ", vret
assert (vret)
def demo():
import numpy
init()
# set initial pose from sample/controller/SampleController/etc/Sample.pos
initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0.637045, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, -0.637045, 0, 0, 0]
hcf.seq_svc.setJointAngles(initial_pose, 0.5)
hcf.seq_svc.waitInterpolation()
# 1. force and moment are large because of link offsets
print numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_rhsensor")).data) > 1e-2
print numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data) > 1e-2
# 2. Set link offsets
# link_offset_centroid and link_offset_mass are identified value.
hcf.rmfo_svc.setForceMomentOffsetParam("rhsensor", OpenHRP.RemoveForceSensorLinkOffsetService.forcemomentOffsetParam(force_offset=[0,0,0], moment_offset=[0,0,0], link_offset_centroid=[0,0.0368,-0.076271], link_offset_mass=0.800011))
hcf.rmfo_svc.setForceMomentOffsetParam("lhsensor", OpenHRP.RemoveForceSensorLinkOffsetService.forcemomentOffsetParam(force_offset=[0,0,0], moment_offset=[0,0,0], link_offset_centroid=[0,-0.0368,-0.076271], link_offset_mass=0.800011))
ret = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")
if ret[0] and ret[1].link_offset_mass == 0.800011:
print "getGaitGeneratorParam() => OK"
ret = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")
if ret[0] and ret[1].link_offset_mass == 0.800011:
print "getGaitGeneratorParam() => OK"
# 3. force and moment are reduced
print numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data) < 1e-2
print numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data) < 1e-2
def calcCOP():
cop_info = rtm.readDataPort(hcf.st.port("COPInfo")).data
lcopx = cop_info[1] / cop_info[2]
lcopy = cop_info[0] / cop_info[2]
rcopx = cop_info[1 + 3] / cop_info[2 + 3]
rcopy = cop_info[0 + 3] / cop_info[2 + 3]
return [
[lcopx, lcopx],
[rcopx, rcopy], # l cop, r cop
[
(cop_info[1] + cop_info[1 + 3]) / (cop_info[2] + cop_info[2 + 3]),
(cop_info[0] + cop_info[0 + 3]) / (cop_info[2] + cop_info[2 + 3]),
],
] # total ZMP
def testOneLimitTable (self_jointId, target_jointId, limit_table, target_llimit, target_ulimit, angle_violation, debug=True, loop_mod=1, thre=1e-2):
tmp_pose=map(lambda x : x, initial_pose)
ret=[]
for idx in range(int(target_ulimit-target_llimit+1)):
if idx%loop_mod != 0: # skip if loop_mod is specified
continue
if debug:
print "idx=",idx,
# A-1. set safe joint
tmp_pose[target_jointId]=deg2rad(target_llimit + idx);
tmp_pose[self_jointId]=deg2rad(limit_table[idx]);
if idx == 0:
hcf.seq_svc.setJointAngles(tmp_pose, 1);
else:
hcf.seq_svc.setJointAngles(tmp_pose, 0.01);
hcf.seq_svc.waitInterpolation()
# A-2. check joint limit is not violated
el_out1 = rtm.readDataPort(hcf.el.port("q")).data
ret1 = abs(rad2deg(el_out1[self_jointId])-limit_table[idx]) < thre and abs(rad2deg(el_out1[target_jointId])- (target_llimit + idx)) < thre
# B-1. set violated joint
tmp_pose[self_jointId]=deg2rad(limit_table[idx]+angle_violation);
hcf.seq_svc.setJointAngles(tmp_pose, 0.01);
hcf.seq_svc.waitInterpolation()
# B-2. check joint limit is not violated
el_out2=rtm.readDataPort(hcf.el.port("q")).data
ret2 = abs(rad2deg(el_out2[self_jointId]) - limit_table[int(round(rad2deg(el_out2[target_jointId])-target_llimit))]) < thre # Check self and target is on limit table
ret2 = ret2 and abs(el_out2[self_jointId] - (limit_table[idx]+angle_violation)) > thre # Check result value is not violated value
# C. results
if debug:
print " ret = (", ret1, ",", ret2,")"
print " self=(o1=", rad2deg(el_out1[self_jointId]), ", o2=", rad2deg(el_out2[self_jointId]), ", limit=", limit_table[idx], ") ", " target=(o1=", rad2deg(el_out1[target_jointId]), ", o2=", rad2deg(el_out2[target_jointId]), ", limit=", target_llimit + idx, ") [deg]"
ret.append(ret1);
ret.append(ret2);
hcf.seq_svc.waitInterpolation()
hcf.seq_svc.setJointAngles(initial_pose, 1);
hcf.seq_svc.waitInterpolation()
return all(ret)
def objectTurnaroundDetection(max_time = 4.0, max_ref_force = 9.8*6.0, limbs=["rarm", "larm"], axis=[0,0,-1]):
otdp=hcf.ic_svc.getObjectTurnaroundDetectorParam()[1]
otdp.axis=axis
hcf.ic_svc.setObjectTurnaroundDetectorParam(otdp)
if limbs==['rarm']:
force = [axis[0]*max_ref_force, axis[1]*max_ref_force, axis[2]*max_ref_force]
hcf.seq_svc.setWrenches([0]*18+force+[0,0,0], max_time)
else:
force = [axis[0]*max_ref_force*0.5, axis[1]*max_ref_force*0.5, axis[2]*max_ref_force*0.5]
hcf.seq_svc.setWrenches([0]*12+force+[0]*3+force+[0]*3, max_time)
hcf.ic_svc.startObjectTurnaroundDetection(max_ref_force, max_time+2.0, limbs)
flg = True
while flg:
tmpflg = hcf.ic_svc.checkObjectTurnaroundDetection()
print rtm.readDataPort(hcf.rmfo.port("off_rhsensor")).data, rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data
print " flag = ", tmpflg, ", forces = ", hcf.ic_svc.getObjectForcesMoments()[1][0], ", moments = ", hcf.ic_svc.getObjectForcesMoments()[2][0]
flg = not tmpflg
time.sleep(0.5)
print " flag = ", tmpflg, ", forces = ", hcf.ic_svc.getObjectForcesMoments()[1][0], ", moments = ", hcf.ic_svc.getObjectForcesMoments()[2][0]
if limbs==['rarm']:
hcf.seq_svc.setWrenches([0]*18+hcf.ic_svc.getObjectForcesMoments()[1][0]+hcf.ic_svc.getObjectForcesMoments()[2][0], 2.0)
else:
hcf.seq_svc.setWrenches([0]*12+hcf.ic_svc.getObjectForcesMoments()[1][0]+hcf.ic_svc.getObjectForcesMoments()[2][0]+hcf.ic_svc.getObjectForcesMoments()[1][1]+hcf.ic_svc.getObjectForcesMoments()[2][1], 2.0)
hcf.waitInterpolation()
def demoGaitGeneratorFixHand():
print >> sys.stderr, "14. Fix arm walking"
hcf.stopAutoBalancer()
hcf.startAutoBalancer()
# Set pose
abcp = hcf.abc_svc.getAutoBalancerParam()[1]
abcp.default_zmp_offsets = [
[0.01, 0.0, 0.0], [0.01, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]
] # Setting default_zmp_offsets is not necessary for fix mode. Just for debugging for default_zmp_offsets in hand fix mode.
hcf.abc_svc.setAutoBalancerParam(abcp)
dualarm_push_pose = [
-3.998549e-05, -0.710564, -0.000264, 1.41027, -0.680767, -2.335251e-05,
-0.541944, -0.091558, 0.122667, -1.02616, -1.71287, -0.056837, 1.5708,
-3.996804e-05, -0.710511, -0.000264, 1.41016, -0.680706, -2.333505e-05,
-0.542, 0.091393, -0.122578, -1.02608, 1.71267, -0.05677, -1.5708,
0.006809, 0.000101, -0.000163
]
hcf.seq_svc.setJointAngles(dualarm_push_pose, 1.0)
hcf.waitInterpolation()
print >> sys.stderr, " Walk without fixing arm"
abcp = hcf.abc_svc.getAutoBalancerParam()[1]
abcp.is_hand_fix_mode = False
hcf.abc_svc.setAutoBalancerParam(abcp)
hcf.abc_svc.goPos(0.3, 0, 0)
hcf.abc_svc.waitFootSteps()
hcf.abc_svc.goPos(0, 0.2, 0)
hcf.abc_svc.waitFootSteps()
hcf.abc_svc.goPos(0, 0, 30)
hcf.abc_svc.waitFootSteps()
print >> sys.stderr, " Walk with fixing arm"
abcp = hcf.abc_svc.getAutoBalancerParam()[1]
abcp.is_hand_fix_mode = True
hcf.abc_svc.setAutoBalancerParam(abcp)
hcf.abc_svc.goPos(0.3, 0, 0)
hcf.abc_svc.waitFootSteps()
hcf.abc_svc.goPos(0, -0.2, 0)
hcf.abc_svc.waitFootSteps()
hcf.abc_svc.goPos(0, 0, -30)
hcf.abc_svc.waitFootSteps()
abcp = hcf.abc_svc.getAutoBalancerParam()[1]
abcp.is_hand_fix_mode = False
abcp.default_zmp_offsets = [[0.0, 0.0, 0.0], [0.0, 0.0, 0.0],
[0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]
hcf.abc_svc.setAutoBalancerParam(abcp)
ref_rpy = rtm.readDataPort(hcf.abc.port("baseRpyOut")).data
hcf.stopAutoBalancer()
checkActualBaseAttitude(ref_rpy)
print >> sys.stderr, " Fix hand=>OK"
def setAndCheckJointVelocityLimit (joint_name, thre=1e-5, dt=0.002):
link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0]
# lvlimit and uvlimit existence check
if not(len(link_info.lvlimit) == 1 and len(link_info.uvlimit) == 1):
print >> sys.stderr, " ", joint_name, " test neglected because no lvlimit and uvlimit are found."
return
for is_upper_limit in [True, False]: # uvlimit or lvlimit
print >> sys.stderr, " ", joint_name, ", uvlimit" if is_upper_limit else ", lvlimit"
# Disable error limit for checking vel limit
hcf.el_svc.setServoErrorLimit("all", 100000)
# Test motion and logging
hcf.clearLog()
target_angle = (math.degrees( link_info.ulimit[0] if is_upper_limit else link_info.llimit[0] )*0.99) # 0.99 is margin
vel_limit = link_info.uvlimit[0] if is_upper_limit else link_info.lvlimit[0]
wait_time = abs(target_angle/math.degrees(vel_limit) * 1.1) # 1.1 is margin
hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.1)
hcf.waitInterpolation()
hcf.setJointAngle(joint_name, target_angle, 0.002)
hcf.waitInterpolation()
hcf.setJointAngle(joint_name, target_angle, wait_time) # Wait for finishing of joint motion
hcf.waitInterpolation()
hcf.saveLog("/tmp/test-samplerobot-el-vel-check")
# Check whether joint angle is reached
reach_angle = math.degrees(rtm.readDataPort(hcf.el.port("q")).data[link_info.jointId])
is_reached = abs(reach_angle - target_angle) < thre
# Check actual velocity from Datalogger log
poslist=[]
for line in open("/tmp/test-samplerobot-el-vel-check.SampleRobot(Robot)0_q", "r"):
poslist.append(float(line.split(" ")[link_info.jointId+1]))
tmp = map(lambda x,y : x-y, poslist[1:], poslist[0:-1])
max_ret_vel = max(tmp)/dt if is_upper_limit else min(tmp)/dt
is_vel_limited = abs(max_ret_vel - vel_limit) < thre
# Enable error limit by reverting limit value and reset joint angle
hcf.el_svc.setServoErrorLimit("all", (0.2-0.02))
hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.5)
hcf.waitInterpolation()
# Check flags and print
print >> sys.stderr, " is_reached =", is_reached, ", is_vel_limited =", is_vel_limited,
print >> sys.stderr, ", target_angle =", target_angle, "[deg], reach_angle =", reach_angle, "[deg], max_ret_vel =", max_ret_vel, "[rad/s], vel_limit =", vel_limit, "[rad/s]"
assert(is_reached and is_vel_limited)
def demoGaitGeneratorFixHand():
print >> sys.stderr, "14. Fix arm walking"
hcf.stopAutoBalancer()
hcf.startAutoBalancer()
# Set pose
abcp=hcf.abc_svc.getAutoBalancerParam()[1]
abcp.default_zmp_offsets=[[0.01, 0.0, 0.0], [0.01, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]] # Setting default_zmp_offsets is not necessary for fix mode. Just for debugging for default_zmp_offsets in hand fix mode.
hcf.abc_svc.setAutoBalancerParam(abcp)
dualarm_push_pose=[-3.998549e-05,-0.710564,-0.000264,1.41027,-0.680767,-2.335251e-05,-0.541944,-0.091558,0.122667,-1.02616,-1.71287,-0.056837,1.5708,-3.996804e-05,-0.710511,-0.000264,1.41016,-0.680706,-2.333505e-05,-0.542,0.091393,-0.122578,-1.02608,1.71267,-0.05677,-1.5708,0.006809,0.000101,-0.000163]
hcf.seq_svc.setJointAngles(dualarm_push_pose, 1.0)
hcf.waitInterpolation()
print >> sys.stderr, " Walk without fixing arm"
abcp=hcf.abc_svc.getAutoBalancerParam()[1]
abcp.is_hand_fix_mode=False
hcf.abc_svc.setAutoBalancerParam(abcp)
hcf.abc_svc.goPos(0.3,0,0)
hcf.abc_svc.waitFootSteps()
hcf.abc_svc.goPos(0,0.2,0)
hcf.abc_svc.waitFootSteps()
hcf.abc_svc.goPos(0,0,30)
hcf.abc_svc.waitFootSteps()
print >> sys.stderr, " Walk with fixing arm"
abcp=hcf.abc_svc.getAutoBalancerParam()[1]
abcp.is_hand_fix_mode=True
hcf.abc_svc.setAutoBalancerParam(abcp)
hcf.abc_svc.goPos(0.3,0,0)
hcf.abc_svc.waitFootSteps()
hcf.abc_svc.goPos(0,-0.2,0)
hcf.abc_svc.waitFootSteps()
hcf.abc_svc.goPos(0,0,-30)
hcf.abc_svc.waitFootSteps()
abcp=hcf.abc_svc.getAutoBalancerParam()[1]
abcp.is_hand_fix_mode=False
abcp.default_zmp_offsets=[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]]
hcf.abc_svc.setAutoBalancerParam(abcp)
ref_rpy = rtm.readDataPort(hcf.abc.port("baseRpyOut")).data
hcf.stopAutoBalancer()
checkActualBaseAttitude(ref_rpy)
print >> sys.stderr, " Fix hand=>OK"
def setAndCheckJointErrorLimit (joint_name, thre=1e-5):
link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0]
for is_upper_limit in [True, False]: # uvlimit or lvlimit
print >> sys.stderr, " ", joint_name, ", uvlimit" if is_upper_limit else ", lvlimit"
# Disable error limit for checking vel limit
error_limit = 1.0 if is_upper_limit else -1.0 # [deg]
hcf.el_svc.setServoErrorLimit("all", abs(math.radians(error_limit)))
# Test motion and logging
hcf.clearLog()
target_angle = 3.0 if is_upper_limit else -3.0 # [deg]
wait_time = abs(target_angle/error_limit * 1.1) # 1.1 is margin
hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.1)
hcf.waitInterpolation()
hcf.setJointAngle(joint_name, target_angle, 0.002)
hcf.waitInterpolation()
hcf.setJointAngle(joint_name, target_angle, wait_time) # Wait for finishing of joint motion
hcf.waitInterpolation()
hcf.saveLog("/tmp/test-samplerobot-el-err-check")
# Check whether joint angle is reached
reach_angle = math.degrees(rtm.readDataPort(hcf.el.port("q")).data[link_info.jointId])
is_reached = abs(reach_angle - target_angle) < thre
# Check actual velocity from Datalogger log
poslist=[]
for line in open("/tmp/test-samplerobot-el-err-check.SampleRobot(Robot)0_q", "r"):
poslist.append(float(line.split(" ")[link_info.jointId+1]))
tmp = map(lambda x,y : x-y, poslist[1:], poslist[0:-1])
max_ret_err = max(tmp) if is_upper_limit else min(tmp)
is_err_limited = abs(max_ret_err - math.radians(error_limit)) < thre
# Enable error limit by reverting limit value and reset joint angle
hcf.el_svc.setServoErrorLimit("all", (0.2-0.02))
hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.5)
hcf.waitInterpolation()
# Check flags and print
print >> sys.stderr, " is_reached =", is_reached, ", is_err_limited =", is_err_limited,
print >> sys.stderr, ", target_angle =", target_angle, "[deg], reach_angle =", reach_angle, "[deg], max_ret_err =", max_ret_err, "[rad], err_limit =", math.radians(error_limit), "[rad]"
assert(is_reached and is_err_limited)
def checkWaist(var_doc):
bpos=rtm.readDataPort(hcf.sh.port("basePosOut")).data
brpy=rtm.readDataPort(hcf.sh.port("baseRpyOut")).data
print " waist => ", checkArrayEquality([bpos.x, bpos.y, bpos.z, brpy.r, brpy.p, brpy.y], var_doc['waist'])
def checkZmp(var_doc):
zmp=rtm.readDataPort(hcf.sh.port("zmpOut")).data
print " zmp => ", checkArrayEquality([zmp.x, zmp.y, zmp.z], var_doc['zmp'])
def checkJointAngles (var_doc):
print " pos => ", checkArrayEquality(rtm.readDataPort(hcf.sh.port("qOut")).data, var_doc['pos'])
def getWrenchArray():
return reduce(
lambda x, y: x + y,
(map(lambda fs: rtm.readDataPort(hcf.es.port(fs + "Out")).data,
['lfsensor', 'rfsensor', 'lhsensor', 'rhsensor'])))
def checkActualBaseAttitude(thre=5.0):
rpy = rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation
ret = abs(math.degrees(rpy.r)) < thre and abs(math.degrees(rpy.p)) < thre
print >> sys.stderr, " actual base rpy = ", ret, "(", rpy, ")"
return ret
def checkTorque (var_doc):
ret = checkArrayEquality(rtm.readDataPort(hcf.sh.port("tqOut")).data, var_doc['torque'])
print " torque => ", ret
assert(ret is True)
def checkActualBaseAttitude(thre=5.0):
rpy = rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation
ret = abs(math.degrees(rpy.r)) < thre and abs(math.degrees(rpy.p)) < thre
print >> sys.stderr, " actual base rpy = ", ret, "(", rpy, ")"
return ret
def checkActualBaseAttitude():
rpy = rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation
ret = math.degrees(rpy.r) < 0.1 and math.degrees(rpy.p) < 0.1
print >> sys.stderr, " actual base rpy = ", ret, "(", rpy, ")"
assert (ret)
return ret
def getWrenchArray ():
return reduce(lambda x,y: x+y, (map(lambda fs : rtm.readDataPort(hcf.es.port(fs+"Out")).data, ['lfsensor', 'rfsensor', 'lhsensor', 'rhsensor'])))
def checkOptionalData(var_doc):
ret = checkArrayEquality(
rtm.readDataPort(hcf.sh.port("optionalDataOut")).data,
var_doc['optionaldata'])
print " optionaldata => ", ret
assert (ret is True)
def checkJointAnglesBetween(from_doc, to_doc):
p0 = from_doc if isinstance(from_doc, list) else from_doc['pos']
p1 = to_doc if isinstance( to_doc, list) else to_doc['pos']
ret = checkArrayBetween(p0, rtm.readDataPort(hcf.sh.port("qOut")).data, p1)
print " pos => ", ret
assert(ret is True)
def demoStandingPosResetting():
print >> sys.stderr, "demoStandingPosResetting"
hcf.abc_svc.goPos(0,0,math.degrees(-1*rtm.readDataPort(hcf.rh.port("WAIST")).data.orientation.y));
hcf.abc_svc.waitFootSteps()
hcf.abc_svc.goPos(0,-1*rtm.readDataPort(hcf.rh.port("WAIST")).data.position.y,0);
hcf.abc_svc.waitFootSteps()
def checkZmp(var_doc):
zmp=rtm.readDataPort(hcf.sh.port("zmpOut")).data
ret = checkArrayEquality([zmp.x, zmp.y, zmp.z], var_doc['zmp'])
print " zmp => ", ret
assert(ret is True)
def checkTorque (var_doc):
print " torque => ", checkArrayEquality(rtm.readDataPort(hcf.sh.port("tqOut")).data, var_doc['torque'])
def checkOptionalData (var_doc):
ret = checkArrayEquality(rtm.readDataPort(hcf.sh.port("optionalDataOut")).data, var_doc['optionaldata'])
print " optionaldata => ", ret
assert(ret is True)
def checkWrenches (var_doc):
print " wrenches => ", checkArrayEquality(reduce(lambda x,y:x+y, map(lambda fs : rtm.readDataPort(hcf.sh.port(fs+"Out")).data, ['lfsensor', 'rfsensor', 'lhsensor', 'rhsensor'])), var_doc['wrenches'])
def checkTorque (var_doc):
ret = checkArrayEquality(rtm.readDataPort(hcf.sh.port("tqOut")).data, var_doc['torque'])
print " torque => ", ret
assert(ret is True)