def test(self):
mb1, mbc1Init = arms.makeZXZArm(True, sva.PTransformd(sva.RotZ(-math.pi/4), eigen.Vector3d(-0.5, 0, 0)))
rbdyn.forwardKinematics(mb1, mbc1Init)
rbdyn.forwardVelocity(mb1, mbc1Init)
mb2, mbc2Init = arms.makeZXZArm(False, sva.PTransformd(sva.RotZ(math.pi/2), eigen.Vector3d(0.5, 0, 0)))
rbdyn.forwardKinematics(mb2, mbc2Init)
rbdyn.forwardVelocity(mb2, mbc2Init)
if not LEGACY:
mbs = rbdyn.MultiBodyVector([mb1, mb2])
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbs = [mb1, mb2]
mbcs = [rbdyn.MultiBodyConfig(mbc1Init), rbdyn.MultiBodyConfig(mbc2Init)]
solver = tasks.qp.QPSolver()
if not LEGACY:
posture1Task = tasks.qp.PostureTask(mbs, 0, mbc1Init.q, 0.1, 10)
posture2Task = tasks.qp.PostureTask(mbs, 1, mbc2Init.q, 0.1, 10)
else:
posture1Task = tasks.qp.PostureTask(mbs, 0, rbdList(mbc1Init.q), 2, 1)
posture2Task = tasks.qp.PostureTask(mbs, 1, rbdList(mbc2Init.q), 2, 1)
mrtt = tasks.qp.MultiRobotTransformTask(mbs, 0, 1, "b3", "b3", sva.PTransformd(sva.RotZ(-math.pi/8)), sva.PTransformd.Identity(), 100, 1000)
if not LEGACY:
mrtt.dimWeight(eigen.VectorXd(0, 0, 1, 1, 1, 0))
else:
mrtt.dimWeight(eigen.Vector6d(0, 0, 1, 1, 1, 0))
solver.addTask(posture1Task)
solver.addTask(posture2Task)
solver.addTask(mrtt)
solver.nrVars(mbs, [], [])
solver.updateConstrSize()
# 3 dof + 9 dof
self.assertEqual(solver.nrVars(), 3 + 9)
for i in range(2000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
self.assertAlmostEqual(mrtt.eval().norm(), 0, delta = 1e-3)
solver.removeTask(posture1Task)
solver.removeTask(posture2Task)
solver.removeTask(mrtt)
def test(self):
mb1, mbc1Init = arms.makeZXZArm(True, sva.PTransformd(sva.RotZ(-math.pi/4), eigen.Vector3d(-0.5, 0, 0)))
rbdyn.forwardKinematics(mb1, mbc1Init)
rbdyn.forwardVelocity(mb1, mbc1Init)
mb2, mbc2Init = arms.makeZXZArm(False, sva.PTransformd(sva.RotZ(math.pi/2), eigen.Vector3d(0.5, 0, 0)))
rbdyn.forwardKinematics(mb2, mbc2Init)
rbdyn.forwardVelocity(mb2, mbc2Init)
if not LEGACY:
mbs = rbdyn.MultiBodyVector([mb1, mb2])
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbs = [mb1, mb2]
mbcs = [rbdyn.MultiBodyConfig(mbc1Init), rbdyn.MultiBodyConfig(mbc2Init)]
solver = tasks.qp.QPSolver()
if not LEGACY:
posture1Task = tasks.qp.PostureTask(mbs, 0, mbc1Init.q, 0.1, 10)
posture2Task = tasks.qp.PostureTask(mbs, 1, mbc2Init.q, 0.1, 10)
else:
posture1Task = tasks.qp.PostureTask(mbs, 0, rbdList(mbc1Init.q), 2, 1)
posture2Task = tasks.qp.PostureTask(mbs, 1, rbdList(mbc2Init.q), 2, 1)
mrtt = tasks.qp.MultiRobotTransformTask(mbs, 0, 1, "b3", "b3", sva.PTransformd(sva.RotZ(-math.pi/8)), sva.PTransformd.Identity(), 100, 1000)
if not LEGACY:
mrtt.dimWeight(eigen.VectorXd(0, 0, 1, 1, 1, 0))
else:
mrtt.dimWeight(eigen.Vector6d(0, 0, 1, 1, 1, 0))
solver.addTask(posture1Task)
solver.addTask(posture2Task)
solver.addTask(mrtt)
solver.nrVars(mbs, [], [])
solver.updateConstrSize()
# 3 dof + 9 dof
self.assertEqual(solver.nrVars(), 3 + 9)
for i in range(2000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
self.assertAlmostEqual(mrtt.eval().norm(), 0, delta = 1e-3)
solver.removeTask(posture1Task)
solver.removeTask(posture2Task)
solver.removeTask(mrtt)
def update(self, rs):
hrp4W_q = rbdList(self.hrp4W.mbc.q)
for i, name in enumerate(rs.joint_name):
if self.hrp4W.hasJoint(name):
hrp4W_q[self.hrp4W.jointIndexByName(name)] = [
rs.joint_position[i]
]
# if name=='L_ANKLE_P':
# print 'L_ANKLE_P'
# print hrp4W_q[self.hrp4W.jointIndexByName(name)]
# print rs.joint_position[i]
# quat=Quaterniond(rs.imu_orientation.w, rs.imu_orientation.x, rs.imu_orientation.y, rs.imu_orientation.z)
# quat.normalize()
# rot=quat.toRotationMatrix()
# acc=rot*Vector3d(rs.imu_linear_acceleration.x,rs.imu_linear_acceleration.y,rs.imu_linear_acceleration.z)-Vector3d(0,0,9.81)
# self.q0Pos=self.q0Pos+0.005*self.q0Vel
# self.q0Vel=self.q0Vel+0.005*acc
# hrp4W_q[0] = [0.9962, 0., 0.0872, tx, 1, tz]
# hrp4W_q[0] = [rs.imu_orientation.w, rs.imu_orientation.x, rs.imu_orientation.y, rs.imu_orientation.z, tx, 1., tz]
hrp4W_q[0] = [
rs.imu_orientation.w, rs.imu_orientation.x, rs.imu_orientation.y,
rs.imu_orientation.z, self.q0Pos[0], self.q0Pos[1], self.q0Pos[2]
]
# hrp4W_q[0] = [0.996194698091746, 0, -0.087155742747658, 0, tx, 1, tz]#self.q0Pos[2]]
# hrp4W_q[0] = [0.9848, 0, 0, 0.1736, self.q0Pos[0], self.q0Pos[1], self.q0Pos[2]]
if self.isDebug:
# hrp4W_q[0] = [0.996, 0, -0.259, 0, self.q0Pos[0], self.q0Pos[1], self.q0Pos[2]]
# hrp4W_q[0] = [0.996, 0, 0, -0.259, self.q0Pos[0], self.q0Pos[1], self.q0Pos[2]]
# hrp4W_q[0] = [0.933, -0.0670, -0.250, 0.250, self.q0Pos[0], self.q0Pos[1], self.q0Pos[2]]
quat = Quaterniond(1, 0, 0, 0)
# quat=Quaterniond(rs.imu_orientation.w, rs.imu_orientation.x, rs.imu_orientation.y, rs.imu_orientation.z)*quat
# quat=Quaterniond(0.965925826289068, 0, 0, -0.258819045102521)*quat
# quat=Quaterniond(0.999377106785000,-0.003316122395400,0.000945268338669,-0.035121335878400)*quat
quat = Quaterniond(0.997375740497000, -0.001451862084460,
-0.008628711870370, 0.071868419320500) * quat
hrp4W_q[0] = [
quat.w(),
quat.x(),
quat.y(),
quat.z(), self.q0Pos[0], self.q0Pos[1], self.q0Pos[2]
]
else:
hrp4W_q[0] = [
rs.imu_orientation.w, rs.imu_orientation.x,
rs.imu_orientation.y, rs.imu_orientation.z, self.q0Pos[0],
self.q0Pos[1], self.q0Pos[2]
]
self.hrp4W.mbc.q = hrp4W_q
# print rbdList(self.hrp4W.mbc.q)[self.hrp4W.jointIndexByName('R_ANKLE_P')]
self.display_helper()
def run(self, rs):
if self.graspCB is not None and self.graspCB.check():
print 'grasping pose'
self.graspCB = None
self.isRunning = True
self.offset_X_box_hd = self.offset_X_box_hd_grasp
if self.stopCB is not None and self.stopCB.check():
print 'stopping'
self.stopCB = None
self.isRunning = True
self.hsCB = stopMotion(robots, qpsolver, postureTask1, None, rbdList(romeo.mbc.q))
try:
PORT = 9559
robotIP = "198.18.0.1"
motionProxy = ALProxy("ALMotion", robotIP, PORT)
except Exception,e:
print "Could not create proxy to ALMotion"
print "Error was: ",e
sys.exit(1)
# Close the hand
handName = 'RHand'
#motionProxy.closeHand(handName)
motionProxy.setStiffnesses(handName, 1.0)
angle = 0.10
fractionMaxSpeed = 0.2
motionProxy.setAngles(handName, angle, fractionMaxSpeed)
time.sleep(1.0)
# Motion to raise the box
chainName = "RArm"
frame = 0 # TORSO
useSensor = True
# Get the current position of the chainName in the same frame
current = motionProxy.getPosition(chainName, frame, useSensor)
target = [
current[0] + 0.00,
current[1] + 0.00,
current[2] + 0.09,
current[3] + 0.0,
current[4] + 0.0,
current[5] + 0.0]
fractionMaxSpeed = 0.3
axisMask = 7 # just control position
motionProxy.setPositions(chainName, frame, target, fractionMaxSpeed, axisMask)
time.sleep(1.0)
self.fsm = self.waitHS
def addUncertainties(self):
"""
Adds bias and noise to the relevant portions
TODO:
- more modular bias
- something more generic that can handle the free-flyer change properly
- noise
"""
new_posture = rbdList(romeo_real.mbc.q)
# new_posture[self.joint_names.index('CHEST_P')] = [new_posture[self.joint_names.index('CHEST_P')][0] - 0.5]
new_posture[self.joint_names.index('RShoulderPitch')] = [new_posture[self.joint_names.index('RShoulderPitch')][0] - 0.5]
# new_posture[self.joint_names.index('R_SHOULDER_R')] = [new_posture[self.joint_names.index('R_SHOULDER_R')][0] + 0.3]
romeo_real.mbc.q = new_posture
def run(self, rs):
if self.stopCB is not None and self.stopCB.check():
print 'stopping'
self.stopCB = None
self.isRunning = True
self.hsCB = stopMotion(robots, qpsolver, postureTask1, None, rbdList(romeo.mbc.q))
self.fsm = self.waitHS
if self.isRunning:
if not qpsolver.run():
print 'FAIL !!!'
self.isRunning = False
return
curTime = rs.header.stamp
romeoJsp.publish(curTime)
qpsolver.send(curTime)
self.fsm(rs)
def __init__(self, robot):
self.hrp4W = robotCopy(robot)
self.hrp4WDisplayer = RobotDisplayer(self.hrp4W, '2')
self.q0Vel = Vector3d().Zero()
# self.q0Pos=Vector3d(-0.021915977704131524, 0.0, 0.7473573364829554)+Vector3d(0,1.0,0)
self.q0Pos = Vector3d(0, 1.0, 0.7473573364829554)
rbd.forwardKinematics(self.hrp4W.mb, self.hrp4W.mbc)
self.hrp4W.mbc.q = robot.mbc.q
hrp4W_q = rbdList(self.hrp4W.mbc.q)
hrp4W_q[0] = [
1., 0., 0., 0., self.q0Pos[0], self.q0Pos[1], self.q0Pos[2]
]
self.hrp4W.mbc.q = hrp4W_q
self.display_helper()
self.isDebug = False
def run(self, rs):
'qp duration'
self.time_qp = time.time() - self.start
start = time.time()
if self.stopCB is not None and self.stopCB.check():
print 'stopping'
self.stopCB = None
self.isRunning = True
self.hsCB = stopMotion(robots, qpsolver, postureTask1, None,
rbdList(hrp4.mbc.q))
self.fsm = self.waitHS
if self.isRunning:
if not qpsolver.run():
print 'FAIL !!!'
self.isRunning = False
return
curTime = rs.header.stamp
# update the center of mass state
rbd.forwardAcceleration(hrp4.mb, hrp4.mbc)
self.com = rbd.computeCoM(hrp4.mb, hrp4.mbc)
self.comA = rbd.computeCoMAcceleration(hrp4.mb, hrp4.mbc)
hrp4W.update(rs)
# print [rs.imu_orientation.w, rs.imu_orientation.x, rs.imu_orientation.y, rs.imu_orientation.z]
# print rbdList(hrp4.mbc.q)[0]
# print rbdList(hrp4W.mbc.bodyPosW)[0].rotation()
# hrp4_q=rbdList(hrp4.mbc.q)
# print hrp4_q[0]
# hrp4W_q=rbdList(hrp4W.hrp4W.mbc.q)
# print hrp4W_q[0]
# i=1
# name=rs.joint_name[i]
# if hrp4.hasJoint(name):
# print hrp4_q[hrp4.jointIndexByName(name)]
# print rs.joint_position[i]
# print hrp4_q[hrp4.jointIndexByName(name)][0]-rs.joint_position[i]
# hrp4_q=rbdList(hrp4.mbc.q)
# for i, name in enumerate(rs.joint_name):
# if hrp4.hasJoint(name):
# print name
# print hrp4_q[hrp4.jointIndexByName(name)]
# print rs.joint_position[i]
# print hrp4_q[hrp4.jointIndexByName(name)][0]-rs.joint_position[i]
if self.fsm == self.wPGiteration:
# Update ZMP to be published
self.zmp_d = Vector3d(self.playbackBridge.zmp_des[0],
self.playbackBridge.zmp_des[1], 0.0)
# markers for debugging the walking pattern generator
if self.isWPGMarkerPublished:
self.zmp_actual = rbd.computeCentroidalZMP(
hrp4.mbc, self.com, self.comA, 0.)
# TODO: use the new API for this!
#compute capture point:
omega = np.sqrt(
9.81 /
self.playbackBridge.robot_params.com_height_)
# omega = np.sqrt(9.81/rbd.computeCoM(hrp4.mb, hrp4.mbc)[2])
comVel = rbd.computeCoMVelocity(hrp4.mb, hrp4.mbc)
capturePoint = self.com + (comVel / omega)
capturePoint[2] = 0.0
# robotH = hrp4
# bodyIdxR = robotH.bodyIndexByName('r_ankle')
# posR=(list(robotH.mbc.bodyPosW)[bodyIdxR]).translation()
# rotR=(list(robotH.mbc.bodyPosW)[bodyIdxR]).rotation()
#
# bodyIdxL = robotH.bodyIndexByName('l_ankle')
# posL=(list(robotH.mbc.bodyPosW)[bodyIdxL]).translation()
# rotL=(list(robotH.mbc.bodyPosW)[bodyIdxL]).rotation()
# walking pattern generator RViZ markers
wpg_markers.fill(
self.zmp_actual, self.zmp_d, self.com,
self.playbackBridge.comRefPos, [
self.playbackBridge.nextStepPos[0],
self.playbackBridge.nextStepPos[1], 0.0
], capturePoint, self.playbackBridge.
LoopControlHelper.zmpPosWanted_,
self.playbackBridge.LoopControlHelper.
zmpPosWantedSatur_, self.playbackBridge.
LoopControlHelper.comPosMesured, self.
playbackBridge.LoopControlHelper.comVelMesured,
self.playbackBridge.LoopControlHelper.
comAccMesured, self.playbackBridge.
LoopControlHelper.zmpPosDesired, self.
playbackBridge.LoopControlHelper.zmpPosMesured,
self.playbackBridge.LoopControlHelper.
zmpRPosMesured, self.playbackBridge.
LoopControlHelper.zmpLPosMesured, self.
playbackBridge.LoopControlHelper.zmpRPosDesired,
self.playbackBridge.LoopControlHelper.
zmpLPosDesired, self.playbackBridge.
LoopControlHelper.zmpRPosWantedSatur_,
self.playbackBridge.LoopControlHelper.
zmpLPosWantedSatur_, self.playbackBridge.
LoopControlHelper.footRPosMesured_, self.
playbackBridge.LoopControlHelper.footLPosMesured_,
self.playbackBridge.ROriMarkerMesured,
self.playbackBridge.LOriMarkerMesured,
self.playbackBridge.ROriMarkerWanted,
self.playbackBridge.LOriMarkerWanted, self.
playbackBridge.LoopControlHelper.zmpPosMesured_,
self.playbackBridge.LoopControlHelper.
zmpRPosMesured_, self.playbackBridge.
LoopControlHelper.zmpLPosMesured_, self.
playbackBridge.LoopControlHelper.comPosMesured_,
self.playbackBridge.LoopControlHelper.DeltaDisplR_,
self.playbackBridge.LoopControlHelper.DeltaDisplL_,
self.playbackBridge.LoopControlHelper.
comPosDesired, self.playbackBridge.
LoopControlHelper.footRForceMesured_,
self.playbackBridge.LoopControlHelper.
footLForceMesured_,
self.playbackBridge.LoopControlHelper.DeltaAnglR_,
self.playbackBridge.LoopControlHelper.DeltaAnglL_,
self.playbackBridge.LoopControlHelper.
zmpRForceWanted_, self.playbackBridge.
LoopControlHelper.zmpLForceWanted_, self.
playbackBridge.LoopControlHelper.comVelMesured_,
self.playbackBridge.LoopControlHelper.
comVelDesired, self.playbackBridge.
LoopControlHelper.capturePointMesured_, self.
playbackBridge.LoopControlHelper.zmpRPosWanted_,
self.playbackBridge.LoopControlHelper.
zmpLPosWanted_, self.playbackBridge.
LoopControlHelper.footRTorquMesured_,
self.playbackBridge.LoopControlHelper.
footLTorquMesured_, self.playbackBridge.
LoopControlHelper.int_delta_comPos)
wpg_markers.publish()
zmp_com_markers.fill(
self.playbackBridge.LoopControlHelper.
comPosDesired,
self.playbackBridge.LoopControlHelper.
zmpPosDesired,
# self.playbackBridge.LoopControlHelper.comPosMesured_,
# self.playbackBridge.LoopControlHelper.zmpPosMesured_,
Vector3d(self.time_qp, self.time_run,
self.time_playback),
Vector3d(
self.playbackBridge.LoopControlHelper.
stateType, self.playbackBridge.stateType, self.
playbackBridge.LoopControlHelper.forceDistrib))
zmp_com_markers.publish()
# Publish all
# hrp4Stab.publishZMPDesired(curTime, self.zmp_d)
hrp4Stab.publish(curTime, self.com, self.comA)
hrp4Jsp.publish(curTime)
qpsolver.fillResult()
q_posture = list(
chain.from_iterable(list(postureTask1.posture())))
qpsolver.qpRes.robots_state.append(Robot(q_posture, [], []))
q_posture = list(postureTask1.eval())
qpsolver.qpRes.robots_state.append(Robot(q_posture, [], []))
# print len(postureTask1.eval())
# print len(rbdList(postureTask1.posture()))
# print len(rbdList(hrp4.mbc.q))
# print hrp4.mb.nrDof()
qpsolver.send(curTime)
self.fsm(rs)
# if not ((self.fsm == self.wait_init_position) or (self.fsm == self.prepareWPG)):
# raw_input('wait user input')
'callrun duration'
self.time_run = time.time() - start
self.start = time.time()
robots = loadRobots()
for r in robots.robots:
r.mbc.gravity = Vector3d(0., 0., 9.81)
hrp4_index = 0
env_index = 1
hrp4 = robots.robots[hrp4_index]
env = robots.robots[env_index]
# compute foot position to be in contact with the ground
rbd.forwardKinematics(hrp4.mb, hrp4.mbc)
tz = -hrp4.surfaces['LeftFoot'].X_0_s(hrp4).translation().z()
tx = -hrp4.surfaces['LeftFoot'].X_0_s(
hrp4).translation().x() #zero the feet surface for the wPG
hrp4_q = rbdList(hrp4.mbc.q)
hrp4_q[0] = [1., 0., 0., 0., tx, 0., tz]
hrp4.mbc.q = hrp4_q
# print len(rbdList(hrp4.mbc.q))
# compute init fk and fv
for r in robots.robots:
rbd.forwardKinematics(r.mb, r.mbc)
rbd.forwardVelocity(r.mb, r.mbc)
hrp4Jsp = JointStatePublisher(hrp4)
# create stabilizer helper
hrp4Stab = stabilizerMsg(hrp4)
# create solver
# load the robot and the environment
robots = loadRobots()
for r in robots.robots:
r.mbc.gravity = Vector3d(0., 0., 9.81)
romeo_index = 0
env_index = 1
romeo = robots.robots[romeo_index]
env = robots.robots[env_index]
# compute foot position to be in contact with the ground
rbd.forwardKinematics(romeo.mb, romeo.mbc)
tz = -romeo.surfaces['Lfoot'].X_0_s(romeo).translation().z()
tx = -romeo.surfaces['Lfoot'].X_0_s(romeo).translation().x() #zero the feet surface
romeo_q = rbdList(romeo.mbc.q)
romeo_q[0] = [1., 0., 0., 0., tx, 0., tz]
romeo.mbc.q = romeo_q
# compute init fk and fv
for r in robots.robots:
rbd.forwardKinematics(r.mb, r.mbc)
rbd.forwardVelocity(r.mb, r.mbc)
romeoJsp = JointStatePublisher(romeo)
# create solver
qpsolver = MRQPSolver(robots, timeStep)
# add dynamics constraint to QPSolver
def test(self):
mb1, mbc1Init = arms.makeZXZArm(
True, sva.PTransformd(eigen.Vector3d(-0.5, 0, 0)))
rbdyn.forwardKinematics(mb1, mbc1Init)
rbdyn.forwardVelocity(mb1, mbc1Init)
mb2, mbc2Init = arms.makeZXZArm(
True, sva.PTransformd(eigen.Vector3d(0.5, 0, 0)))
rbdyn.forwardKinematics(mb2, mbc2Init)
rbdyn.forwardVelocity(mb2, mbc2Init)
if not LEGACY:
X_0_b1 = sva.PTransformd(mbc1Init.bodyPosW[-1])
X_0_b2 = sva.PTransformd(mbc2Init.bodyPosW[-1])
else:
X_0_b1 = sva.PTransformd(list(mbc1Init.bodyPosW)[-1])
X_0_b2 = sva.PTransformd(list(mbc2Init.bodyPosW)[-1])
X_b1_b2 = X_0_b2 * X_0_b1.inv()
if not LEGACY:
mbs = rbdyn.MultiBodyVector([mb1, mb2])
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbs = [mb1, mb2]
mbcs = [
rbdyn.MultiBodyConfig(mbc1Init),
rbdyn.MultiBodyConfig(mbc2Init)
]
nrGen = 3
solver = tasks.qp.QPSolver()
contVec = [
tasks.qp.UnilateralContact(0, 1, "b3", "b3",
[eigen.Vector3d.Zero()],
sva.RotX(math.pi / 2), X_b1_b2, nrGen,
0.7)
]
if not LEGACY:
posture1Task = tasks.qp.PostureTask(mbs, 0, mbc1Init.q, 2, 1)
posture2Task = tasks.qp.PostureTask(mbs, 1, mbc2Init.q, 2, 1)
else:
posture1Task = tasks.qp.PostureTask(mbs, 0, rbdList(mbc1Init.q), 2,
1)
posture2Task = tasks.qp.PostureTask(mbs, 1, rbdList(mbc2Init.q), 2,
1)
comD = (rbdyn.computeCoM(mb1, mbc1Init) + rbdyn.computeCoM(
mb2, mbc2Init)) / 2 + eigen.Vector3d(0, 0, 0.5)
multiCoM = tasks.qp.MultiCoMTask(mbs, [0, 1], comD, 10, 500)
multiCoM.updateInertialParameters(mbs)
contCstrSpeed = tasks.qp.ContactSpeedConstr(0.001)
solver.addTask(posture1Task)
solver.addTask(posture2Task)
solver.nrVars(mbs, contVec, [])
solver.addTask(mbs, multiCoM)
contCstrSpeed.addToSolver(mbs, solver)
solver.updateConstrSize()
self.assertEqual(solver.nrVars(), 3 + 3 + 1 * nrGen)
for i in range(2000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
# Check that the link hold
if not LEGACY:
X_0_b1_post = mbcs[0].bodyPosW[-1]
X_0_b2_post = mbcs[1].bodyPosW[-1]
else:
X_0_b1_post = list(mbcs[0].bodyPosW)[-1]
X_0_b2_post = list(mbcs[1].bodyPosW)[-1]
X_b1_b2_post = X_0_b2 * X_0_b1.inv()
self.assertAlmostEqual(
(X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(),
0,
delta=1e-5)
self.assertAlmostEqual(multiCoM.speed().norm(), 0, delta=1e-3)
contCstrSpeed.removeFromSolver(solver)
solver.removeTask(posture1Task)
solver.removeTask(posture2Task)
solver.removeTask(multiCoM)
def test(self):
mb1, mbc1Init = arms.makeZXZArm()
rbdyn.forwardKinematics(mb1, mbc1Init)
rbdyn.forwardVelocity(mb1, mbc1Init)
mb2, mbc2Init = arms.makeZXZArm(False)
if not LEGACY:
mb2InitPos = mbc1Init.bodyPosW[-1].translation()
else:
mb2InitPos = list(mbc1Init.bodyPosW)[-1].translation()
mb2InitOri = eigen.Quaterniond(sva.RotY(math.pi / 2))
if not LEGACY:
mbc2Init.q[0] = [
mb2InitOri.w(),
mb2InitOri.x(),
mb2InitOri.y(),
mb2InitOri.z(),
mb2InitPos.x(),
mb2InitPos.y() + 1,
mb2InitPos.z()
]
mbc2Init.q[0] = [
mb2InitOri.w(),
mb2InitOri.x(),
mb2InitOri.y(),
mb2InitOri.z(),
mb2InitPos.x(),
mb2InitPos.y() + 1,
mb2InitPos.z()
]
rbdyn.forwardKinematics(mb2, mbc2Init)
rbdyn.forwardVelocity(mb2, mbc2Init)
if not LEGACY:
X_0_b1 = sva.PTransformd(mbc1Init.bodyPosW[-1])
X_0_b2 = sva.PTransformd(mbc2Init.bodyPosW[-1])
else:
X_0_b1 = sva.PTransformd(list(mbc1Init.bodyPosW)[-1])
X_0_b2 = sva.PTransformd(list(mbc2Init.bodyPosW)[-1])
X_b1_b2 = X_0_b2 * X_0_b1.inv()
if not LEGACY:
mbs = rbdyn.MultiBodyVector([mb1, mb2])
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbs = [mb1, mb2]
mbcs = [
rbdyn.MultiBodyConfig(mbc1Init),
rbdyn.MultiBodyConfig(mbc2Init)
]
# Test ContactAccConstr constraint and PositionTask on the second robot
solver = tasks.qp.QPSolver()
points = [
eigen.Vector3d(0.1, 0, 0.1),
eigen.Vector3d(0.1, 0, -0.1),
eigen.Vector3d(-0.1, 0, -0.1),
eigen.Vector3d(-0.1, 0, 0.1),
]
biPoints = [
eigen.Vector3d.Zero(),
eigen.Vector3d.Zero(),
eigen.Vector3d.Zero(),
eigen.Vector3d.Zero(),
]
nrGen = 4
biFrames = [
sva.RotX(math.pi / 4),
sva.RotX(3 * math.pi / 4),
sva.RotX(math.pi / 4) * sva.RotY(math.pi / 2),
sva.RotX(3 * math.pi / 4) * sva.RotY(math.pi / 2),
]
# The fixed robot can pull the other
contVecFail = [
tasks.qp.UnilateralContact(0, 1, "b3", "b0", points,
sva.RotX(-math.pi / 2), X_b1_b2, nrGen,
0.7)
]
# The fixed robot can push the other
contVec = [
tasks.qp.UnilateralContact(0, 1, "b3", "b0", points,
sva.RotX(math.pi / 2), X_b1_b2, nrGen,
0.7)
]
# The fixed robot has non coplanar force apply on the other
contVecBi = [
tasks.qp.BilateralContact(tasks.qp.ContactId(0, 1, "b3", "b0"),
biPoints, biFrames, X_b1_b2, nrGen, 1)
]
if not LEGACY:
posture1Task = tasks.qp.PostureTask(mbs, 0, mbc1Init.q, 2, 1)
posture2Task = tasks.qp.PostureTask(mbs, 1, mbc2Init.q, 2, 1)
else:
posture1Task = tasks.qp.PostureTask(mbs, 0, rbdList(mbc1Init.q), 2,
1)
posture2Task = tasks.qp.PostureTask(mbs, 1, rbdList(mbc2Init.q), 2,
1)
contCstrSpeed = tasks.qp.ContactSpeedConstr(0.001)
Inf = float("inf")
torqueMin1 = [[], [-Inf], [-Inf], [-Inf]]
torqueMax1 = [[], [Inf], [Inf], [Inf]]
torqueMin2 = [[0, 0, 0, 0, 0, 0], [-Inf], [-Inf], [-Inf]]
torqueMax2 = [[0, 0, 0, 0, 0, 0], [Inf], [Inf], [Inf]]
motion1 = tasks.qp.MotionConstr(
mbs, 0, tasks.TorqueBound(torqueMin1, torqueMax1))
motion2 = tasks.qp.MotionConstr(
mbs, 1, tasks.TorqueBound(torqueMin2, torqueMax2))
plCstr = tasks.qp.PositiveLambda()
motion1.addToSolver(solver)
motion2.addToSolver(solver)
plCstr.addToSolver(solver)
contCstrSpeed.addToSolver(solver)
solver.addTask(posture1Task)
solver.addTask(posture2Task)
# Check the impossible motion
solver.nrVars(mbs, contVecFail, [])
solver.updateConstrSize()
self.assertEqual(solver.nrVars(), 3 + 9 + 4 * nrGen)
self.assertFalse(solver.solve(mbs, mbcs))
# Check the unilateral motion
if not LEGACY:
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbcs = [
rbdyn.MultiBodyConfig(mbc1Init),
rbdyn.MultiBodyConfig(mbc2Init)
]
solver.nrVars(mbs, contVec, [])
solver.updateConstrSize()
for i in range(1000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
# Check that the link hold
if not LEGACY:
X_0_b1_post = mbcs[0].bodyPosW[-1]
X_0_b2_post = mbcs[1].bodyPosW[-1]
else:
X_0_b1_post = list(mbcs[0].bodyPosW)[-1]
X_0_b2_post = list(mbcs[1].bodyPosW)[-1]
X_b1_b2_post = X_0_b2 * X_0_b1.inv()
self.assertAlmostEqual(
(X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(),
0,
delta=1e-5)
# Force in the world frame must be the same
f1 = contVec[0].force(solver.lambdaVec(0), contVec[0].r1Cone)
f2 = contVec[0].force(solver.lambdaVec(0), contVec[0].r2Cone)
self.assertAlmostEqual((f1 + f2).norm(), 0, delta=1e-5)
# Check the bilateral motion
if not LEGACY:
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbcs = [
rbdyn.MultiBodyConfig(mbc1Init),
rbdyn.MultiBodyConfig(mbc2Init)
]
solver.nrVars(mbs, contVec, [])
solver.updateConstrSize()
self.assertEqual(solver.nrVars(), 3 + 9 + 4 * nrGen)
for i in range(1000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
# Check that the link hold
if not LEGACY:
X_0_b1_post = mbcs[0].bodyPosW[-1]
X_0_b2_post = mbcs[1].bodyPosW[-1]
else:
X_0_b1_post = list(mbcs[0].bodyPosW)[-1]
X_0_b2_post = list(mbcs[1].bodyPosW)[-1]
X_b1_b2_post = X_0_b2 * X_0_b1.inv()
self.assertAlmostEqual(
(X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(),
0,
delta=1e-5)
# Force in the world frame must be the same
f1 = contVec[0].force(solver.lambdaVec(0), contVec[0].r1Cone)
f2 = contVec[0].force(solver.lambdaVec(0), contVec[0].r2Cone)
self.assertAlmostEqual((f1 + f2).norm(), 0, delta=1e-5)
plCstr.removeFromSolver(solver)
motion2.removeFromSolver(solver)
motion1.removeFromSolver(solver)
contCstrSpeed.removeFromSolver(solver)
solver.removeTask(posture1Task)
solver.removeTask(posture2Task)
def run(self, rs):
if self.stopCB is not None and self.stopCB.check():
print 'stopping'
self.stopCB = None
self.isRunning = True
self.hsCB = stopMotion(robots, qpsolver, postureTask1, None,
rbdList(hrp4.mbc.q))
self.fsm = self.waitHS
if self.isRunning:
if not qpsolver.run():
print 'FAIL !!!'
self.isRunning = False
return
curTime = rs.header.stamp
# update the center of mass state
rbd.forwardAcceleration(hrp4.mb, hrp4.mbc)
self.com = rbd.computeCoM(hrp4.mb, hrp4.mbc)
self.comA = rbd.computeCoMAcceleration(hrp4.mb, hrp4.mbc)
if self.fsm == self.wPGiteration:
# Update ZMP to be published
self.zmp_d = Vector3d(self.playbackBridge.zmp_des[0],
self.playbackBridge.zmp_des[1], 0.0)
# markers for debugging the walking pattern generator
if self.isWPGMarkerPublished:
self.zmp_actual = rbd.computeCentroidalZMP(
hrp4.mbc, self.com, self.comA, 0.)
# TODO: use the new API for this!
#compute capture point:
omega = np.sqrt(
9.81 /
self.playbackBridge.robot_params.com_height_)
# omega = np.sqrt(9.81/rbd.computeCoM(hrp4.mb, hrp4.mbc)[2])
comVel = rbd.computeCoMVelocity(hrp4.mb, hrp4.mbc)
capturePoint = self.com + (comVel / omega)
capturePoint[2] = 0.0
# robotH = hrp4
# bodyIdxR = robotH.bodyIndexByName('r_ankle')
# posR=(list(robotH.mbc.bodyPosW)[bodyIdxR]).translation()
# rotR=(list(robotH.mbc.bodyPosW)[bodyIdxR]).rotation()
#
# bodyIdxL = robotH.bodyIndexByName('l_ankle')
# posL=(list(robotH.mbc.bodyPosW)[bodyIdxL]).translation()
# rotL=(list(robotH.mbc.bodyPosW)[bodyIdxL]).rotation()
# walking pattern generator RViZ markers
wpg_markers.fill(
self.zmp_actual, self.zmp_d, self.com,
self.playbackBridge.comRefPos, [
self.playbackBridge.nextStepPos[0],
self.playbackBridge.nextStepPos[1], 0.0
], capturePoint)
wpg_markers.publish()
# Publish all
# hrp4Stab.publishZMPDesired(curTime, self.zmp_d)
hrp4Stab.publish(curTime, self.com, self.comA)
hrp4Jsp.publish(curTime)
qpsolver.send(curTime)
self.fsm(rs)
def callWPG(self, qpsolver, comTask, comTaskTr, torsoOriTask, \
rfPosTaskTr, lfPosTaskTr, rfOriTask, lfOriTask, c1L, c1R,rs):
# raw_input('wait user input')
# self.LoopControlHelper.update(rs)
if self.wPG_iters == 1:
q0 = rbdList(self.LoopControlHelper.robotW.mbc.q)[0]
quatIMU = Quaterniond(q0[0], q0[1], q0[2], q0[3])
quatIMU.normalize()
self.LoopControlHelper.rotInitIMU = quatIMU.toRotationMatrix()
self.LoopControlHelper.rotInitIMUinv = self.LoopControlHelper.rotInitIMU.inverse(
)
self.RFootHelper = task_playback_helper(self.robot, 'r_ankle',
rfPosTaskTr)
self.LFootHelper = task_playback_helper(self.robot, 'l_ankle',
lfPosTaskTr)
# self.RWFootHelper=task_playback_helper(self.robotW, 'r_ankle',rfPosTaskTr)
# self.LWFootHelper=task_playback_helper(self.robotW, 'l_ankle',lfPosTaskTr)
# print self.LFootHelper.bodyState.getOriW()
# print self.LWFootHelper.bodyState.getOriW()
# self.LoopControlHelper.update(rs)
# print 'delta comVel'
# print self.LoopControlHelper.comVelDesired-self.LoopControlHelper.comVelMesured_
# print 'delta comPos'
# print (self.LoopControlHelper.comPosDesired-(self.LoopControlHelper.footRPosFixed+self.LoopControlHelper.footLPosFixed)/2)-(self.LoopControlHelper.comPosMesured_-(self.LoopControlHelper.footRPosMesured_+self.LoopControlHelper.footLPosMesured_)/2)
# print 'delta zmpPos'
# print (self.LoopControlHelper.zmpPosDesired-(self.LoopControlHelper.footRPosFixed+self.LoopControlHelper.footLPosFixed)/2)-(self.LoopControlHelper.zmpPosMesured_-(self.LoopControlHelper.footRPosMesured_+self.LoopControlHelper.footLPosMesured_)/2)
# if self.IsWalkActiv==False:
# self.LoopControlHelper.update(self.robot,self.robotW,
# self.RWFootHelper,self.LWFootHelper,
# rs.wrench[iR],rs.wrench[iL])
# if self.IsWalkActiv:
# nbiter=160
## nbiter=len(self.zmpcom)>0:
# else:
# nbiter=2000
# if self.wPG_iters<nbiter:
if True:
print '\n ========== wPG iteration ', self.wPG_iters
self.wPG_iters += 1
# take the computed com state of the QP
comPos = rbd.computeCoM(self.robot.mb, self.robot.mbc)
# comVel = rbd.computeCoMVelocity(self.robot.mb, self.robot.mbc)
if hasattr(self.LoopControlHelper, 'footRForceMesured'):
# CheckingFloorContact=sqrt(self.LoopControlHelper.footRForceMesured[0]**2+self.LoopControlHelper.footRForceMesured[1]**2+self.LoopControlHelper.footRForceMesured[2]**2+
# (self.LoopControlHelper.footLForceMesured[0]**2+self.LoopControlHelper.footLForceMesured[1]**2+self.LoopControlHelper.footLForceMesured[2]**2))
CheckingFloorContact = sqrt(
(self.LoopControlHelper.footRForceMesured[0] +
self.LoopControlHelper.footLForceMesured[0])**2 +
(self.LoopControlHelper.footRForceMesured[1] +
self.LoopControlHelper.footLForceMesured[1])**2 +
(self.LoopControlHelper.footRForceMesured[2] +
self.LoopControlHelper.footLForceMesured[2])**2)
if CheckingFloorContact < self.LoopControlHelper.M * 9.81 / 2:
self.IsControlLoopActiv = False
if self.IsDebugWalking:
ktoto = 0.05
maxcounterwalk = 500.0
if self.counterWalk > maxcounterwalk:
self.IsWalkActiv = False
# update state with walking pattern generator
if self.IsControlLoopActiv and not self.IsWalkActiv:
self.comRefPos = self.LoopControlHelper.comPosDesired
self.comRefVel = self.LoopControlHelper.comVelDesired
self.comRefAccel = self.LoopControlHelper.comAccDesired
self.zmp_des[0] = self.LoopControlHelper.zmpPosDesired[0]
self.zmp_des[1] = self.LoopControlHelper.zmpPosDesired[1]
self.stateType = 0
if self.counterWalk > maxcounterwalk:
self.comRefPos[1] = ktoto
self.comRefVel[1] = 0
self.comRefAccel[1] = 0
self.zmp_des[1] = ktoto
elif not self.IsControlLoopActiv and self.IsWalkActiv:
# # update state with walking pattern generator
self.comRefPos = self.LoopControlHelper.comPosDesired
self.comRefVel = self.LoopControlHelper.comVelDesired
self.comRefAccel = self.LoopControlHelper.comAccDesired
self.zmp_des[0] = self.LoopControlHelper.zmpPosDesired[0]
self.zmp_des[1] = self.LoopControlHelper.zmpPosDesired[1]
self.stateType = 0
if self.counterWalk is None:
self.counterWalk = 1
if self.counterWalk <= maxcounterwalk:
self.comRefPos[1] = (
1 - cos(self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto
self.comRefVel[1] = (sin(
self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto * np.pi / maxcounterwalk * 0
self.comRefAccel[1] = (cos(
self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto * (np.pi / maxcounterwalk) * (
np.pi / maxcounterwalk) * 0
self.zmp_des[1] = -self.comRefAccel[
1] * 0.78 / 9.81 + self.comRefPos[1]
self.counterWalk += 1
self.LoopControlHelper.comPosDesired[
1] = self.comRefPos[1]
self.LoopControlHelper.zmpPosDesired[1] = self.zmp_des[
1]
elif self.counterWalk > maxcounterwalk:
self.comRefPos[1] = ktoto
self.comRefVel[1] = 0
self.comRefAccel[1] = 0
self.zmp_des[1] = ktoto
self.LoopControlHelper.comPosDesired[
1] = self.comRefPos[1]
self.LoopControlHelper.zmpPosDesired[1] = self.zmp_des[
1]
elif self.IsWalkActiv:
self.comRefPos = self.LoopControlHelper.comPosDesired
self.comRefVel = self.LoopControlHelper.comVelDesired
self.comRefAccel = self.LoopControlHelper.comAccDesired
self.zmp_des[0] = self.LoopControlHelper.zmpPosDesired[0]
self.zmp_des[1] = self.LoopControlHelper.zmpPosDesired[1]
self.stateType = 0
if self.counterWalk is None:
self.counterWalk = 1
if self.counterWalk <= maxcounterwalk:
self.comRefPos[1] = (
1 - cos(self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto
self.comRefVel[1] = (sin(
self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto * np.pi / maxcounterwalk * 0
self.comRefAccel[1] = (cos(
self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto * (np.pi / maxcounterwalk) * (
np.pi / maxcounterwalk) * 0
self.zmp_des[1] = -self.comRefAccel[
1] * 0.78 / 9.81 + self.comRefPos[1]
self.counterWalk += 1
self.LoopControlHelper.comPosDesired[
1] = self.comRefPos[1]
self.LoopControlHelper.zmpPosDesired[1] = self.zmp_des[
1]
elif self.counterWalk > maxcounterwalk:
self.comRefPos[1] = ktoto
self.comRefVel[1] = 0
self.comRefAccel[1] = 0
self.zmp_des[1] = ktoto
self.LoopControlHelper.comPosDesired[
1] = self.comRefPos[1]
self.LoopControlHelper.zmpPosDesired[1] = self.zmp_des[
1]
else:
self.comRefPos = self.LoopControlHelper.comPosDesired
self.comRefVel = self.LoopControlHelper.comVelDesired
self.comRefAccel = self.LoopControlHelper.comAccDesired
self.zmp_des[0] = self.LoopControlHelper.zmpPosDesired[0]
self.zmp_des[1] = self.LoopControlHelper.zmpPosDesired[1]
self.stateType = 0
if self.counterWalk > maxcounterwalk:
self.comRefPos[1] = ktoto
self.comRefVel[1] = 0
self.comRefAccel[1] = 0
self.zmp_des[1] = ktoto
self.LoopControlHelper.comPosDesired[
1] = self.comRefPos[1]
self.LoopControlHelper.zmpPosDesired[1] = self.zmp_des[
1]
else:
ktoto = 0.0
maxcounterwalk = 500.0
if self.IsWalkActiv and len(self.zmpcom) > 0:
zmp_com_now = self.zmpcom.pop(0)
else:
self.IsWalkActiv = False
# update state with walking pattern generator
if self.IsControlLoopActiv and not self.IsWalkActiv:
# self.comRefPos = self.LoopControlHelper.comPosWanted
# self.comRefVel = self.LoopControlHelper.comVelWanted
# self.comRefAccel = self.LoopControlHelper.comAccWanted
self.comRefPos = self.LoopControlHelper.comPosDesired + Vector3d(
ktoto, 0, 0)
self.LoopControlHelper.comVelDesired = Vector3d().Zero()
self.LoopControlHelper.comAccDesire = Vector3d().Zero()
self.comRefVel = self.LoopControlHelper.comVelDesired
self.comRefAccel = self.LoopControlHelper.comAccDesired
# self.zmp_des[0] = self.LoopControlHelper.zmpPosWanted[0]
# self.zmp_des[1] = self.LoopControlHelper.zmpPosWanted[1]
# self.zmp_des[0] = self.LoopControlHelper.zmpPosDesired[0]+ktoto
# self.zmp_des[1] = self.LoopControlHelper.zmpPosDesired[1]
self.zmp_des[0] = self.comRefPos[0]
self.zmp_des[1] = self.comRefPos[1]
# self.stateType = 0
elif self.IsControlLoopActiv and self.IsWalkActiv:
# update state with walking pattern generator
self.LoopControlHelper.comPosDesired = Vector3d(
zmp_com_now[4] + ktoto, zmp_com_now[5], 0.780678)
self.LoopControlHelper.comVelDesired = Vector3d(
zmp_com_now[7], zmp_com_now[8], zmp_com_now[9])
self.LoopControlHelper.comAccDesired = Vector3d(
zmp_com_now[10], zmp_com_now[11], zmp_com_now[12])
self.LoopControlHelper.previousStateType = self.stateType
self.LoopControlHelper.stateType = zmp_com_now[0]
self.LoopControlHelper.zmpPosDesired[
0] = zmp_com_now[1] + ktoto
self.LoopControlHelper.zmpPosDesired[1] = zmp_com_now[2]
self.comRefPos = Vector3d(zmp_com_now[4] + ktoto,
zmp_com_now[5], 0.780678)
self.comRefVel = Vector3d(zmp_com_now[7], zmp_com_now[8],
zmp_com_now[9])
self.comRefAccel = Vector3d(zmp_com_now[10],
zmp_com_now[11],
zmp_com_now[12])
self.previousStateType = self.stateType
self.stateType = zmp_com_now[0] #0=TDS, 1=LSS, 2=RSS
self.zmp_des[0] = zmp_com_now[1] + ktoto
self.zmp_des[1] = zmp_com_now[2]
elif self.IsWalkActiv:
self.LoopControlHelper.comPosDesired = Vector3d(
zmp_com_now[4] + ktoto, zmp_com_now[5], 0.780678)
self.LoopControlHelper.comVelDesired = Vector3d(
zmp_com_now[7], zmp_com_now[8], zmp_com_now[9])
self.LoopControlHelper.comAccDesired = Vector3d(
zmp_com_now[10], zmp_com_now[11], zmp_com_now[12])
self.LoopControlHelper.previousStateType = self.stateType
self.LoopControlHelper.stateType = zmp_com_now[0]
self.LoopControlHelper.zmpPosDesired[
0] = zmp_com_now[1] + ktoto
self.LoopControlHelper.zmpPosDesired[1] = zmp_com_now[2]
# update state with walking pattern generator
self.comRefPos = Vector3d(zmp_com_now[4] + ktoto,
zmp_com_now[5], 0.780678)
self.comRefVel = Vector3d(zmp_com_now[7], zmp_com_now[8],
zmp_com_now[9])
self.comRefAccel = Vector3d(zmp_com_now[10],
zmp_com_now[11],
zmp_com_now[12])
self.previousStateType = self.stateType
self.stateType = zmp_com_now[0] #0=TDS, 1=LSS, 2=RSS
self.zmp_des[0] = zmp_com_now[1] + ktoto
self.zmp_des[1] = zmp_com_now[2]
else:
self.comRefPos = self.LoopControlHelper.comPosDesired
self.LoopControlHelper.comVelDesired = Vector3d().Zero()
self.LoopControlHelper.comAccDesire = Vector3d().Zero()
self.comRefVel = self.LoopControlHelper.comVelDesired
self.comRefAccel = self.LoopControlHelper.comAccDesired
self.zmp_des[0] = self.LoopControlHelper.zmpPosDesired[0]
self.zmp_des[1] = self.LoopControlHelper.zmpPosDesired[1]
self.zmp_des[0] = self.comRefPos[0]
self.zmp_des[1] = self.comRefPos[1]
# self.stateType = 0
if self.counterWalk is None:
self.counterWalk = 1
if self.counterWalk <= maxcounterwalk:
self.comRefPos[0] = (
1 - cos(self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto - 0.0036526
self.comRefVel[0] = (sin(
self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto * np.pi / maxcounterwalk * 0
self.comRefAccel[0] = (cos(
self.counterWalk / maxcounterwalk *
np.pi)) / 2 * ktoto * (np.pi / maxcounterwalk) * (
np.pi / maxcounterwalk) * 0
self.zmp_des[0] = -self.comRefAccel[
0] * 0.78 / 9.81 + self.comRefPos[0]
self.counterWalk += 1
self.LoopControlHelper.comPosDesired[
0] = self.comRefPos[0]
self.LoopControlHelper.zmpPosDesired[0] = self.zmp_des[
0]
self.zmpRPosDesired = Vector3d(
self.LoopControlHelper.zmpPosDesired[0],
-0.0815817, 0)
self.zmpLPosDesired = Vector3d(
self.LoopControlHelper.zmpPosDesired[0], 0.0815817,
0)
#TODO: very unsure of this part
if self.IsWalkActiv:
if ((self.stateType != self.previousStateType)
and (self.previousStateType == 0)):
next_pstep = self.pstep.pop(0)
self.nextStepPos = [next_pstep[0], next_pstep[1]]
self.next_step_angle = next_pstep[2]
if self.stateType == 1:
self.previousRfootPosFixed[
0] = self.LoopControlHelper.footRPosFixed[0]
self.previousRfootPosFixed[
1] = self.LoopControlHelper.footRPosFixed[1]
self.LoopControlHelper.footRPosFixed[0] = next_pstep[0]
self.LoopControlHelper.footRPosFixed[1] = next_pstep[1]
self.LoopControlHelper.footRAnglFixed = next_pstep[2]
else:
self.previousLfootPosFixed[
0] = self.LoopControlHelper.footLPosFixed[0]
self.previousLfootPosFixed[
1] = self.LoopControlHelper.footLPosFixed[1]
self.LoopControlHelper.footLPosFixed[0] = next_pstep[0]
self.LoopControlHelper.footLPosFixed[1] = next_pstep[1]
self.LoopControlHelper.footLAnglFixed = next_pstep[2]
'''
'''
if self.wPG_iters == 2:
self.LoopControlHelper.footRPosFixed = Vector3d(
self.zmpcom[0][22], self.zmpcom[0][23], self.zmpcom[0][24])
self.LoopControlHelper.footLPosFixed = Vector3d(
self.zmpcom[0][13], self.zmpcom[0][14], self.zmpcom[0][15])
self.LoopControlHelper.footRAnglFixed = self.zmpcom[0][36]
self.LoopControlHelper.footLAnglFixed = self.zmpcom[0][33]
self.LoopControlHelper.update(rs, self.IsControlLoopActiv)
if self.FootWidthEnlarge:
maxcounterFootEnlarge = 500.0
if self.counterFootEnlarge is None:
self.counterFootEnlarge = 0
elif self.counterFootEnlarge < maxcounterFootEnlarge:
self.counterFootEnlarge += 1
if self.IsWalkActiv or self.IsControlLoopActiv:
self.FootWidthEnlarge = False
# '''
# close loop in Qp targets update of COM and ZMP
# '''
# if self.IsControlLoopActiv and self.comRefPosInit is None:
# self.comRefPosInit=self.comRefPos
# self.comRefVelInit=self.comRefVel
# self.comRefAccelInit=self.comRefAccel
# self.zmp_desInit=self.zmp_des
# elif not self.IsControlLoopActiv:
# self.comRefPosInit=None
# self.comRefVelInit=None
# self.comRefAccelInit=None
# self.zmp_desInit=None
#
# if self.IsControlLoopActiv:
# self.zmp_desInit[0]=self.LoopControlHelper.zmpPosWantedSatur_[0]
# self.zmp_desInit[1]=self.LoopControlHelper.zmpPosWantedSatur_[1]
#
# self.comRefAccelInit=self.LoopControlHelper.zmpForceWanted_/self.LoopControlHelper.M
# self.comRefAccelInit[2]=0
#
# self.comRefPosInit=self.comRefPos+self.comRefVelInit*1/200+self.comRefAccelInit*(1/200)**2/2
#
# self.comRefVelInit=self.comRefVelInit+self.comRefAccelInit*1/200
#
# self.comRefPos=self.comRefPosInit
# self.comRefVel=self.comRefVelInit
# self.comRefAccel=self.comRefAccelInit
# self.zmp_des=self.comRefPosInit
# elif not self.comRefPosInit is None:
# self.comRefPos=self.comRefPosInit
# self.comRefVel=self.comRefVelInit*0
# self.comRefAccel=self.comRefAccelInit*0
# self.zmp_des=self.comRefPosInit
# if self.wPG_iters==2:
# self.comRefPosInit=self.comRefPos
# self.comRefVelInit=self.comRefVel
# self.comRefAccelInit=self.comRefAccel
# self.zmp_desInit=self.zmp_des
# else:
# self.zmp_desInit[0]=self.LoopControlHelper.zmpPosWantedSatur_[0]
# self.zmp_desInit[1]=self.LoopControlHelper.zmpPosWantedSatur_[1]
#
# self.comRefAccelInit=self.LoopControlHelper.zmpForceWanted_/self.LoopControlHelper.M
# self.comRefAccelInit[2]=0
#
# self.comRefVelInit=self.comRefVelInit+self.comRefAccelInit*1/200
# self.comRefPosInit=self.comRefPos+self.comRefVelInit*1/200
#
# self.comRefPos=self.comRefPosInit
# self.comRefVel=self.comRefVelInit
# self.comRefAccel=self.comRefAccelInit
# self.zmp_des=self.zmp_desInit
comTask.com(self.comRefPos)
comTaskTr.refVel(toVecX(self.comRefVel))
comTaskTr.refAccel(toVecX(self.comRefAccel))
# prevents a bug caused by the orientation task dimWeight #TODO: fix properly in Tasks self.initAlpha *
torsoOriTask.orientation(
sva.RotY(0.14) * sva.RotZ(
(self.last_rotation_angle_rfoot +
self.last_rotation_angle_lfoot) / 2.))
# double support
if self.stateType == 0:
print 'state TDS'
if not (self.previousStateType == 0):
qpsolver.setContacts([c1L, c1R])
qpsolver.update()
print '------------updating contact state'
# left single support
elif self.stateType == 1:
self.swingFoot = self.rFoot
print 'state LSS'
if (self.previousStateType == 0):
qpsolver.setContacts([c1L])
qpsolver.update()
print '------------updating contact state'
self.last_rotation_angle_rfoot = self.next_step_angle
self.previousRFootPos = self.RFootHelper.bodyState.getPosW(
)
# right single support
elif self.stateType == 2:
self.swingFoot = self.lFoot
print 'state RSS'
if (self.previousStateType == 0):
qpsolver.setContacts([c1R])
qpsolver.update()
print '------------updating contact state'
self.last_rotation_angle_lfoot = self.next_step_angle
self.previousLFootPos = self.LFootHelper.bodyState.getPosW(
)
'''
Manage Right Foot tasks
'''
if self.IsControlLoopActiv and not self.IsWalkActiv:
# rFootPos=self.LoopControlHelper.footRPosWanted
rFootPos = self.RFootHelper.bodyState.getPosW() + Vector3d(
self.LoopControlHelper.DeltaDisplR_[0],
self.LoopControlHelper.DeltaDisplR_[1],
self.LoopControlHelper.DeltaDisplR_[2]
) #+self.LoopControlHelper.DeltaDisplR_
elif self.IsControlLoopActiv and self.IsWalkActiv:
# rFootPos=Vector3d(zmp_com_now[22],zmp_com_now[23],zmp_com_now[24])
rFootPos = self.RFootHelper.bodyState.getPosW() + Vector3d(
self.LoopControlHelper.DeltaDisplR_[0],
self.LoopControlHelper.DeltaDisplR_[1],
self.LoopControlHelper.DeltaDisplR_[2]
) #+self.LoopControlHelper.DeltaDisplR_
if not self.IsDebugWalking:
if self.stateType == 1:
rFootPos = self.previousRFootPos - self.previousRfootPosFixed + Vector3d(
zmp_com_now[22], zmp_com_now[23], zmp_com_now[24])
else:
rFootPos = self.RFootHelper.bodyState.getPosW(
) + Vector3d(self.LoopControlHelper.DeltaDisplR_[0],
self.LoopControlHelper.DeltaDisplR_[1],
self.LoopControlHelper.DeltaDisplR_[2])
elif self.IsWalkActiv:
# rFootPos=Vector3d(zmp_com_now[22],zmp_com_now[23],zmp_com_now[24])
## rFootPos=Vector3d(zmp_com_now[22],zmp_com_now[23],0.093)
## rFootVel=Vector3d(zmp_com_now[25],zmp_com_now[26],zmp_com_now[27])
## rFootAcc=Vector3d(zmp_com_now[28],zmp_com_now[29],zmp_com_now[30])
rFootPos = self.RFootHelper.bodyState.getPosW(
) #-Vector3d(+0.005,0,0)
if not self.IsDebugWalking:
if self.stateType == 1:
rFootPos = self.previousRFootPos - self.previousRfootPosFixed + Vector3d(
zmp_com_now[22], zmp_com_now[23], zmp_com_now[24])
else:
rFootPos = self.RFootHelper.bodyState.getPosW()
elif self.FootWidthEnlarge:
rFootPos = self.LoopControlHelper.footRPosInit + (
self.LoopControlHelper.footRPosFixed -
self.LoopControlHelper.footRPosInit
) * self.counterFootEnlarge / maxcounterFootEnlarge
else:
# if self.FootWidthEnlarge:
# rFootPos=Vector3d(self.zmpcom[0][22],max(self.RFootHelper.bodyState.getPosW()[1]-0.001,self.zmpcom[0][23]),self.zmpcom[0][24])
# else:
rFootPos = self.RFootHelper.bodyState.getPosW()
# rFootVel=Vector3d(0,0,0)
# rFootAcc=Vector3d(0,0,0)
# self.RFootHelper.update(rFootPos,rFootVel,rFootAcc)
rfPosTaskTr.position(rFootPos)
if self.IsControlLoopActiv:
rfOriTask.orientation(self.LoopControlHelper.DeltafootROri_ *
self.RFootHelper.bodyState.getOriW())
toto3 = self.RFootHelper.bodyState.getOriW()
self.ROriMarkerMesured = toto3.eulerAngles(0, 1, 2)
toto4 = self.LoopControlHelper.DeltafootROri_ * self.RFootHelper.bodyState.getOriW(
)
self.ROriMarkerWanted = toto4.eulerAngles(0, 1, 2)
elif self.IsWalkActiv:
# rfOriTask.orientation(sva.RotX(zmp_com_now[34])*sva.RotY(zmp_com_now[35])*sva.RotZ(zmp_com_now[36]))
rfOriTask.orientation(self.RFootHelper.bodyState.getOriW())
elif self.FootWidthEnlarge:
rfOriTask.orientation(
sva.RotZ(self.LoopControlHelper.footRAnglFixed *
self.counterFootEnlarge / maxcounterFootEnlarge))
else:
# rfOriTask.orientation(sva.RotX(self.LoopControlHelper.footRAnglDesired[0])*sva.RotY(self.LoopControlHelper.footRAnglDesired[1])*sva.RotZ(self.LoopControlHelper.footRAnglDesired[2]))
rfOriTask.orientation(self.RFootHelper.bodyState.getOriW())
'''
Manage Left Foot tasks
'''
# lFootPos=Vector3d(zmp_com_now[13],zmp_com_now[14],zmp_com_now[15])
# lFootVel=Vector3d(zmp_com_now[16],zmp_com_now[17],zmp_com_now[18])
# lFootAcc=Vector3d(zmp_com_now[19],zmp_com_now[20],zmp_com_now[21])
if self.IsControlLoopActiv and not self.IsWalkActiv:
# lFootPos=self.LoopControlHelper.footLPosWanted
lFootPos = self.LFootHelper.bodyState.getPosW() + Vector3d(
self.LoopControlHelper.DeltaDisplL_[0],
self.LoopControlHelper.DeltaDisplL_[1],
self.LoopControlHelper.DeltaDisplL_[2]
) #+self.LoopControlHelper.DeltaDisplR_
elif self.IsControlLoopActiv and self.IsWalkActiv:
# lFootPos=Vector3d(zmp_com_now[13],zmp_com_now[14],zmp_com_now[15])
lFootPos = self.LFootHelper.bodyState.getPosW() + Vector3d(
self.LoopControlHelper.DeltaDisplL_[0],
self.LoopControlHelper.DeltaDisplL_[1],
self.LoopControlHelper.DeltaDisplL_[2]
) #+self.LoopControlHelper.DeltaDisplR_
if not self.IsDebugWalking:
if self.stateType == 2:
lFootPos = self.previousLFootPos - self.previousLfootPosFixed + Vector3d(
zmp_com_now[13], zmp_com_now[14], zmp_com_now[15])
else:
lFootPos = self.LFootHelper.bodyState.getPosW(
) + Vector3d(self.LoopControlHelper.DeltaDisplL_[0],
self.LoopControlHelper.DeltaDisplL_[1],
self.LoopControlHelper.DeltaDisplL_[2])
elif self.IsWalkActiv:
# lFootPos=Vector3d(zmp_com_now[13],zmp_com_now[14],zmp_com_now[15])
## lFootPos=Vector3d(zmp_com_now[13],zmp_com_now[14],0.093)
## lFootVel=Vector3d(zmp_com_now[16],zmp_com_now[17],zmp_com_now[18])
## lFootAcc=Vector3d(zmp_com_now[19],zmp_com_now[20],zmp_com_now[21])
lFootPos = self.LFootHelper.bodyState.getPosW(
) #-Vector3d(-0.005,0,0)
if not self.IsDebugWalking:
if self.stateType == 2:
lFootPos = self.previousLFootPos - self.previousLfootPosFixed + Vector3d(
zmp_com_now[13], zmp_com_now[14], zmp_com_now[15])
else:
lFootPos = self.LFootHelper.bodyState.getPosW()
elif self.FootWidthEnlarge:
lFootPos = self.LoopControlHelper.footLPosInit + (
self.LoopControlHelper.footLPosFixed -
self.LoopControlHelper.footLPosInit
) * self.counterFootEnlarge / maxcounterFootEnlarge
else:
# if self.FootWidthEnlarge:
# lFootPos=Vector3d(self.zmpcom[0][13],min(self.LFootHelper.bodyState.getPosW()[1]+0.001,self.zmpcom[0][14]),self.zmpcom[0][15])
# else:
lFootPos = self.LFootHelper.bodyState.getPosW()
# lFootVel=Vector3d(0,0,0)
# lFootAcc=Vector3d(0,0,0)
# self.LFootHelper.update(lFootPos,lFootVel,lFootAcc)
lfPosTaskTr.position(lFootPos)
if self.IsControlLoopActiv:
lfOriTask.orientation(self.LoopControlHelper.DeltafootLOri_ *
self.LFootHelper.bodyState.getOriW())
toto1 = self.LFootHelper.bodyState.getOriW()
self.LOriMarkerMesured = toto1.eulerAngles(0, 1, 2)
toto2 = self.LoopControlHelper.DeltafootLOri_ * self.LFootHelper.bodyState.getOriW(
)
self.LOriMarkerWanted = toto2.eulerAngles(0, 1, 2)
elif self.IsWalkActiv:
# lfOriTask.orientation(sva.RotX(zmp_com_now[31])*sva.RotY(zmp_com_now[32])*sva.RotZ(zmp_com_now[33]))
lfOriTask.orientation(self.LFootHelper.bodyState.getOriW())
elif self.FootWidthEnlarge:
lfOriTask.orientation(
sva.RotZ(self.LoopControlHelper.footLAnglFixed *
self.counterFootEnlarge / maxcounterFootEnlarge))
else:
# lfOriTask.orientation(sva.RotX(self.LoopControlHelper.footLAnglDesired[0])*sva.RotY(self.LoopControlHelper.footLAnglDesired[1])*sva.RotZ(self.LoopControlHelper.footLAnglDesired[2]))
lfOriTask.orientation(self.LFootHelper.bodyState.getOriW())
# lfOriTask.orientation(sva.RotX(self.LoopControlHelper.footLAnglDesired[0])*sva.RotY(self.LoopControlHelper.footLAnglDesired[1]+0.00175*self.wPG_iters)*sva.RotZ(self.LoopControlHelper.footLAnglDesired[2]))
# used to compare if contact state needs updating
self.previousStateType = self.stateType
# if self.FootWidthEnlarge==True:
# if self.LFootHelper.bodyState.getPosW()[1]+0.001>=self.zmpcom[0][14] and self.RFootHelper.bodyState.getPosW()[1]-0.001<=self.zmpcom[0][23]:
# self.FootWidthEnlarge=False
else:
print 'wPG ended with ', self.wPG_iters - 1, ' iterations'
self.hasEnded = True
def test(self):
mb1, mbc1Init = arms.makeZXZArm(True, sva.PTransformd(eigen.Vector3d(-0.5, 0, 0)))
rbdyn.forwardKinematics(mb1, mbc1Init)
rbdyn.forwardVelocity(mb1, mbc1Init)
mb2, mbc2Init = arms.makeZXZArm(True, sva.PTransformd(eigen.Vector3d(0.5, 0, 0)))
rbdyn.forwardKinematics(mb2, mbc2Init)
rbdyn.forwardVelocity(mb2, mbc2Init)
if not LEGACY:
X_0_b1 = sva.PTransformd(mbc1Init.bodyPosW[-1])
X_0_b2 = sva.PTransformd(mbc2Init.bodyPosW[-1])
else:
X_0_b1 = sva.PTransformd(list(mbc1Init.bodyPosW)[-1])
X_0_b2 = sva.PTransformd(list(mbc2Init.bodyPosW)[-1])
X_b1_b2 = X_0_b2*X_0_b1.inv()
if not LEGACY:
mbs = rbdyn.MultiBodyVector([mb1, mb2])
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbs = [mb1, mb2]
mbcs = [rbdyn.MultiBodyConfig(mbc1Init), rbdyn.MultiBodyConfig(mbc2Init)]
nrGen = 3
solver = tasks.qp.QPSolver()
contVec = [ tasks.qp.UnilateralContact(0, 1, "b3", "b3", [eigen.Vector3d.Zero()], sva.RotX(math.pi/2), X_b1_b2, nrGen, 0.7) ]
if not LEGACY:
posture1Task = tasks.qp.PostureTask(mbs, 0, mbc1Init.q, 2, 1)
posture2Task = tasks.qp.PostureTask(mbs, 1, mbc2Init.q, 2, 1)
else:
posture1Task = tasks.qp.PostureTask(mbs, 0, rbdList(mbc1Init.q), 2, 1)
posture2Task = tasks.qp.PostureTask(mbs, 1, rbdList(mbc2Init.q), 2, 1)
comD = (rbdyn.computeCoM(mb1, mbc1Init) + rbdyn.computeCoM(mb2, mbc2Init))/2 + eigen.Vector3d(0, 0, 0.5)
multiCoM = tasks.qp.MultiCoMTask(mbs, [0,1], comD, 10, 500)
multiCoM.updateInertialParameters(mbs)
contCstrSpeed = tasks.qp.ContactSpeedConstr(0.001)
solver.addTask(posture1Task)
solver.addTask(posture2Task)
solver.nrVars(mbs, contVec, [])
solver.addTask(mbs, multiCoM)
contCstrSpeed.addToSolver(mbs, solver)
solver.updateConstrSize()
self.assertEqual(solver.nrVars(), 3 + 3 + 1*nrGen)
for i in range(2000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
# Check that the link hold
if not LEGACY:
X_0_b1_post = mbcs[0].bodyPosW[-1]
X_0_b2_post = mbcs[1].bodyPosW[-1]
else:
X_0_b1_post = list(mbcs[0].bodyPosW)[-1]
X_0_b2_post = list(mbcs[1].bodyPosW)[-1]
X_b1_b2_post = X_0_b2*X_0_b1.inv()
self.assertAlmostEqual((X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(), 0, delta = 1e-5)
self.assertAlmostEqual(multiCoM.speed().norm(), 0, delta = 1e-3)
contCstrSpeed.removeFromSolver(solver)
solver.removeTask(posture1Task)
solver.removeTask(posture2Task)
solver.removeTask(multiCoM)
def test(self):
mb1, mbc1Init = arms.makeZXZArm()
rbdyn.forwardKinematics(mb1, mbc1Init)
rbdyn.forwardVelocity(mb1, mbc1Init)
mb2, mbc2Init = arms.makeZXZArm(False)
if not LEGACY:
mb2InitPos = mbc1Init.bodyPosW[-1].translation()
else:
mb2InitPos = list(mbc1Init.bodyPosW)[-1].translation()
mb2InitOri = eigen.Quaterniond(sva.RotY(math.pi/2))
if not LEGACY:
mbc2Init.q[0] = [mb2InitOri.w(), mb2InitOri.x(), mb2InitOri.y(), mb2InitOri.z(), mb2InitPos.x(), mb2InitPos.y() + 1, mb2InitPos.z()]
mbc2Init.q[0] = [mb2InitOri.w(), mb2InitOri.x(), mb2InitOri.y(), mb2InitOri.z(), mb2InitPos.x(), mb2InitPos.y() + 1, mb2InitPos.z()]
rbdyn.forwardKinematics(mb2, mbc2Init)
rbdyn.forwardVelocity(mb2, mbc2Init)
if not LEGACY:
X_0_b1 = sva.PTransformd(mbc1Init.bodyPosW[-1])
X_0_b2 = sva.PTransformd(mbc2Init.bodyPosW[-1])
else:
X_0_b1 = sva.PTransformd(list(mbc1Init.bodyPosW)[-1])
X_0_b2 = sva.PTransformd(list(mbc2Init.bodyPosW)[-1])
X_b1_b2 = X_0_b2*X_0_b1.inv()
if not LEGACY:
mbs = rbdyn.MultiBodyVector([mb1, mb2])
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbs = [mb1, mb2]
mbcs = [rbdyn.MultiBodyConfig(mbc1Init), rbdyn.MultiBodyConfig(mbc2Init)]
# Test ContactAccConstr constraint and PositionTask on the second robot
solver = tasks.qp.QPSolver()
points = [
eigen.Vector3d(0.1, 0, 0.1),
eigen.Vector3d(0.1, 0, -0.1),
eigen.Vector3d(-0.1, 0, -0.1),
eigen.Vector3d(-0.1, 0, 0.1),
]
biPoints = [
eigen.Vector3d.Zero(),
eigen.Vector3d.Zero(),
eigen.Vector3d.Zero(),
eigen.Vector3d.Zero(),
]
nrGen = 4
biFrames = [
sva.RotX(math.pi/4),
sva.RotX(3*math.pi/4),
sva.RotX(math.pi/4)*sva.RotY(math.pi/2),
sva.RotX(3*math.pi/4)*sva.RotY(math.pi/2),
]
# The fixed robot can pull the other
contVecFail = [ tasks.qp.UnilateralContact(0, 1, "b3", "b0", points, sva.RotX(-math.pi/2), X_b1_b2, nrGen, 0.7) ]
# The fixed robot can push the other
contVec = [ tasks.qp.UnilateralContact(0, 1, "b3", "b0", points, sva.RotX(math.pi/2), X_b1_b2, nrGen, 0.7) ]
# The fixed robot has non coplanar force apply on the other
contVecBi = [ tasks.qp.BilateralContact(tasks.qp.ContactId(0, 1, "b3", "b0"), biPoints, biFrames, X_b1_b2, nrGen, 1) ]
if not LEGACY:
posture1Task = tasks.qp.PostureTask(mbs, 0, mbc1Init.q, 2, 1)
posture2Task = tasks.qp.PostureTask(mbs, 1, mbc2Init.q, 2, 1)
else:
posture1Task = tasks.qp.PostureTask(mbs, 0, rbdList(mbc1Init.q), 2, 1)
posture2Task = tasks.qp.PostureTask(mbs, 1, rbdList(mbc2Init.q), 2, 1)
contCstrSpeed = tasks.qp.ContactSpeedConstr(0.001)
Inf = float("inf")
torqueMin1 = [[], [-Inf], [-Inf], [-Inf]]
torqueMax1 = [[], [Inf], [Inf], [Inf]]
torqueMin2 = [[0,0,0,0,0,0], [-Inf], [-Inf], [-Inf]]
torqueMax2 = [[0,0,0,0,0,0], [Inf], [Inf], [Inf]]
motion1 = tasks.qp.MotionConstr(mbs, 0, tasks.TorqueBound(torqueMin1, torqueMax1))
motion2 = tasks.qp.MotionConstr(mbs, 1, tasks.TorqueBound(torqueMin2, torqueMax2))
plCstr = tasks.qp.PositiveLambda()
motion1.addToSolver(solver)
motion2.addToSolver(solver)
plCstr.addToSolver(solver)
contCstrSpeed.addToSolver(solver)
solver.addTask(posture1Task)
solver.addTask(posture2Task)
# Check the impossible motion
solver.nrVars(mbs, contVecFail, [])
solver.updateConstrSize()
self.assertEqual(solver.nrVars(), 3 + 9 + 4*nrGen)
self.assertFalse(solver.solve(mbs, mbcs))
# Check the unilateral motion
if not LEGACY:
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbcs = [rbdyn.MultiBodyConfig(mbc1Init), rbdyn.MultiBodyConfig(mbc2Init)]
solver.nrVars(mbs, contVec, [])
solver.updateConstrSize()
for i in range(1000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
# Check that the link hold
if not LEGACY:
X_0_b1_post = mbcs[0].bodyPosW[-1]
X_0_b2_post = mbcs[1].bodyPosW[-1]
else:
X_0_b1_post = list(mbcs[0].bodyPosW)[-1]
X_0_b2_post = list(mbcs[1].bodyPosW)[-1]
X_b1_b2_post = X_0_b2*X_0_b1.inv()
self.assertAlmostEqual((X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(), 0, delta = 1e-5)
# Force in the world frame must be the same
f1 = contVec[0].force(solver.lambdaVec(0), contVec[0].r1Cone)
f2 = contVec[0].force(solver.lambdaVec(0), contVec[0].r2Cone)
self.assertAlmostEqual((f1+f2).norm(), 0, delta = 1e-5)
# Check the bilateral motion
if not LEGACY:
mbcs = rbdyn.MultiBodyConfigVector([mbc1Init, mbc2Init])
else:
mbcs = [rbdyn.MultiBodyConfig(mbc1Init), rbdyn.MultiBodyConfig(mbc2Init)]
solver.nrVars(mbs, contVec, [])
solver.updateConstrSize()
self.assertEqual(solver.nrVars(), 3 + 9 + 4*nrGen)
for i in range(1000):
if not LEGACY:
self.assertTrue(solver.solve(mbs, mbcs))
else:
self.assertTrue(solver.solveNoMbcUpdate(mbs, mbcs))
solver.updateMbc(mbcs[0], 0)
solver.updateMbc(mbcs[1], 1)
for i in range(2):
rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001)
rbdyn.forwardKinematics(mbs[i], mbcs[i])
rbdyn.forwardVelocity(mbs[i], mbcs[i])
# Check that the link hold
if not LEGACY:
X_0_b1_post = mbcs[0].bodyPosW[-1]
X_0_b2_post = mbcs[1].bodyPosW[-1]
else:
X_0_b1_post = list(mbcs[0].bodyPosW)[-1]
X_0_b2_post = list(mbcs[1].bodyPosW)[-1]
X_b1_b2_post = X_0_b2*X_0_b1.inv()
self.assertAlmostEqual((X_b1_b2.matrix() - X_b1_b2_post.matrix()).norm(), 0, delta = 1e-5)
# Force in the world frame must be the same
f1 = contVec[0].force(solver.lambdaVec(0), contVec[0].r1Cone)
f2 = contVec[0].force(solver.lambdaVec(0), contVec[0].r2Cone)
self.assertAlmostEqual((f1+f2).norm(), 0, delta = 1e-5)
plCstr.removeFromSolver(solver)
motion2.removeFromSolver(solver)
motion1.removeFromSolver(solver)
contCstrSpeed.removeFromSolver(solver)
solver.removeTask(posture1Task)
solver.removeTask(posture2Task)
r.mbc.gravity = Vector3d(0., 0., 9.81)
romeo_index = 0
env_index = 1
# romeo_real_index = 2
romeo = robots.robots[romeo_index]
env = robots.robots[env_index]
# romeo_real = robots.robots[romeo_real_index]
# compute foot position to be in contact with the ground
for rob in [romeo]:
rbd.forwardKinematics(rob.mb, rob.mbc)
tz = -rob.surfaces['Lfoot'].X_0_s(rob).translation().z()
tx = -rob.surfaces['Lfoot'].X_0_s(rob).translation().x() #zero the feet surface
rob_q = rbdList(rob.mbc.q)
rob_q[0] = [1., 0., 0., 0., tx, 0., tz]
rob.mbc.q = rob_q
if rob is romeo:
romeo_q = rob_q
# compute init fk and fv
for r in robots.robots:
rbd.forwardKinematics(r.mb, r.mbc)
rbd.forwardVelocity(r.mb, r.mbc)
romeoJsp = JointStatePublisher(romeo)
#romeo_real_Jsp = JointStatePublisher(romeo_real)
# create solver