base_estimator.set_left_foot_sizes(conf.LEFT_FOOT_SIZES)
base_estimator.q.recompute(0)
print(base_estimator.q.value)
print(len(base_estimator.q.value))
# --- Reference frame
rf = SimpleReferenceFrame('rf')
rf.init(robot_name)
rf.footLeft.value = leftPos.homogeneous.tolist()
rf.footRight.value = rightPos.homogeneous.tolist()
# --- State transformation
stf = StateTransformation("stf")
stf.init()
plug(rf.referenceFrame, stf.referenceFrame)
plug(base_estimator.q, stf.q_in)
plug(base_estimator.v, stf.v_in)
stf.q.recompute(0)
print(stf.q.value)
print(len(stf.q.value))
stf.v.recompute(0)
print(stf.v.value)
print(len(stf.v.value))
# --- Conversion ---
print("--- Conversion ---")
e2q = EulerToQuat('e2q')
base_estimator.set_normal_force_margin_left_foot(conf.normal_force_margin)
base_estimator.set_right_foot_sizes(conf.RIGHT_FOOT_SIZES)
base_estimator.set_left_foot_sizes(conf.LEFT_FOOT_SIZES)
base_estimator.q.recompute(0)
print(base_estimator.q.value)
print(len(base_estimator.q.value))
base_estimator.v.recompute(0)
print(base_estimator.v.value)
print(len(base_estimator.v.value))
# --- Conversion ---
print("--- Conversion ---")
e2q = EulerToQuat('e2q')
plug(base_estimator.q, e2q.euler)
e2q.quaternion.recompute(0)
print(e2q.quaternion.value)
print(len(e2q.quaternion.value))
q_est = np.matrix(e2q.quaternion.value).T
# --- Raw q difference ---
print("--- Raw q difference ---")
q_diff = q_est - q
print(q_diff.flatten().tolist()[0])
# --- Estimated feet ---
print("--- Estimated feet ---")
data2 = model.createData()
pin.framesForwardKinematics(model, data2, q_est)
print(data2.oMf[rightId])
dcm_controller.omega.value = omega
dcm_controller.com.value = comDes
dcm_controller.dcm.value = comDes
dcm_controller.zmpDes.value = zmpDes
dcm_controller.dcmDes.value = dcmDes
dcm_controller.init(dt)
# --- Wrench distribution ---
print("--- Wrench distribution ---")
distribute = create_distribute_wrench(distribute_conf)
distribute.q.value = halfSitting
plug(dcm_controller.wrenchRef, distribute.wrenchDes)
distribute.init(robot_name)
distribute.zmpRef.recompute(0)
print("reference wrench: %s" % str(dcm_controller.wrenchRef.value))
assertApprox(wrench, dcm_controller.wrenchRef.value, 3)
print("resulting wrench: %s" % str(distribute.wrenchRef.value))
assertApprox(wrench, distribute.wrenchRef.value, 2)
print("resulting left wrench: %s" % str(distribute.wrenchLeft.value))
assertApprox(wrenchLeft, distribute.wrenchLeft.value, 3)
print("resulting right wrench: %s" % str(distribute.wrenchRight.value))
assertApprox(wrenchRight, distribute.wrenchRight.value, 3)
# --- Reference frame ---
print("--- Reference frame ---")
rf = SimpleReferenceFrame('rf')
rf.init(robot_name)
rf.footLeft.value = leftPos.homogeneous.tolist()
rf.footRight.value = rightPos.homogeneous.tolist()
rf.reset.value = 1
# --- Dummy Walking Pattern Generator ---
print("--- Dummy Walking Pattern Generator ---")
wp = DummyWalkingPatternGenerator('dummy_wp')
wp.init()
plug(rf.referenceFrame, wp.referenceFrame)
wp.omega.value = omega
wp.footLeft.value = leftPos.homogeneous.tolist()
wp.footRight.value = rightPos.homogeneous.tolist()
wp.com.value = com
wp.vcom.value = vcom
wp.acom.value = acom
wp.comDes.recompute(0)
wp.dcmDes.recompute(0)
wp.zmpDes.recompute(0)
# --- Output
print("Desired CoM:")
print(wp.comDes.value)
assertApprox(comDes, wp.comDes.value, 3)