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)