# --- Parameter server --- print("--- Parameter server ---") param_server = create_parameter_server(param_server_conf, dt) # --- DCM controller Kp_dcm = [0.0, 0.0, 0.0] Ki_dcm = [0.0, 0.0, 0.0] gamma_dcm = 0.2 dcm_controller = DcmController("dcmCtrl") dcm_controller.Kp.value = Kp_dcm dcm_controller.Ki.value = Ki_dcm dcm_controller.decayFactor.value = gamma_dcm dcm_controller.mass.value = m 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) dcm_controller.wrenchRef.recompute(0) print("reference wrench: %s" % str(dcm_controller.wrenchRef.value)) assertApprox(wrench, dcm_controller.wrenchRef.value, 3)
from numpy.testing import assert_almost_equal as assertApprox from sot_talos_balance.dummy_dcm_estimator import DummyDcmEstimator dummy = DummyDcmEstimator('dummy') dummy.mass.value = 1.0 dummy.omega.value = 1.0 dummy.com.value = [1.] * 3 dummy.momenta.value = [2.] * 3 dummy.init() dummy.dcm.recompute(0) print(dummy.dcm.value) assertApprox(dummy.dcm.value, [3.] * 3) dummy.momenta.value = [3.] * 6 dummy.dcm.recompute(1) print(dummy.dcm.value) assertApprox(dummy.dcm.value, [4.] * 3)
print("--- Conversion ---") e2q = EulerToQuat('e2q') plug(stf.q, e2q.euler) e2q.quaternion.recompute(0) print(e2q.quaternion.value) print(len(e2q.quaternion.value)) q_est = np.matrix(e2q.quaternion.value).T # temp if q_est[6] < 0 and q[6] > 0: print("Warning: quaternions have different signs") q_est[3:7] = -q_est[3:7] print(q_est.T) assertApprox(q, q_est, 3) # --- Raw q difference --- print("--- Raw q difference ---") q_diff = q_est - q print(q_diff.flatten().tolist()[0]) # --- DCM estimator --- print("--- DCM estimator ---") dcm_estimator = DcmEstimator('dcm_estimator') dcm_estimator.init(dt, robot_name) plug(e2q.quaternion, dcm_estimator.q) plug(base_estimator.v, dcm_estimator.v) dcm_estimator.c.recompute(0) print(dcm_estimator.c.value)
print("wrenchLeft: %s" % (estimator.wrenchLeft.value, )) print("wrenchRight: %s" % (estimator.wrenchRight.value, )) print("poseLeft:\n%s" % (np.matrix(estimator.poseLeft.value), )) print("poseRight:\n%s" % (np.matrix(estimator.poseRight.value), )) estimator.init() estimator.zmp.recompute(0) copLeft = (1.0, 0.0, 0.0) copRight = (0.0, 1.0, 0.0) zmp = (0.5, 0.5, 0.0) print() print("copLeft: %s" % (estimator.copLeft.value, )) assertApprox(estimator.copLeft.value, copLeft) print("copRight: %s" % (estimator.copRight.value, )) assertApprox(estimator.copRight.value, copRight) print("zmp: %s" % (estimator.zmp.value, )) assertApprox(estimator.zmp.value, zmp) # --- Test emergency stop print() print("--- Test emergency stop ---") print() estimator.emergencyStop.recompute(0) stop = estimator.emergencyStop.value print("emergencyStop: %d" % stop) np.testing.assert_equal(stop, 0)
lfw = conf.lfw ftc.setLeftFootWeight(lfw) ftc.setRightFootWeight(rfw) print('Weights of both feet set to {0}'.format(rfw)) print('The robot should be in the air.') ftc.right_foot_force_in.value = [1, 1, 8, 1, 1, 1] ftc.left_foot_force_in.value = [1, 1, 6, 1, 1, 1] print("Let's calibrate the ft sensors...") ftc.calibrateFeetSensor() for i in range(2, 1003): ftc.right_foot_force_in.value = np.random.randn(6) * 0.0000001 + [ 1, 1, 8, 1, 1, 1 ] ftc.left_foot_force_in.value = np.random.randn(6) * 0.0000001 + [ 1, 1, 6, 1, 1, 1 ] ftc.right_foot_force_out.recompute(i) ftc.left_foot_force_out.recompute(i) assertApprox(ftc.right_foot_force_out.value, np.array((0, 0, -rfw, 0, 0, 0)), 5) assertApprox(ftc.left_foot_force_out.value, np.array((0, 0, -lfw, 0, 0, 0)), 5) print("Ft sensors calibrated!") print("Value outputed after calibration:") print(ftc.right_foot_force_out.value) print(ftc.left_foot_force_out.value)
print("expected global left wrench: %s" % str(wrenchLeft)) print("expected global right wrench: %s" % str(wrenchRight)) print("expected ankle left wrench: %s" % str(ankleWrenchLeft)) print("expected ankle right wrench: %s" % str(ankleWrenchRight)) copLeft = [float(com[0] - leftPos.translation[0]), 0., 0.] copRight = [float(com[0] - rightPos.translation[0]), 0., 0.] print("expected sole left CoP: %s" % str(copLeft)) print("expected sole right CoP: %s" % str(copRight)) print() distribute.zmpRef.recompute(0) print("resulting global wrench: %s" % str(distribute.wrenchRef.value)) assertApprox(wrench, distribute.wrenchRef.value, 2) print("resulting global left wrench: %s" % str(distribute.wrenchLeft.value)) assertApprox(wrenchLeft, distribute.wrenchLeft.value, 3) print("resulting global right wrench: %s" % str(distribute.wrenchRight.value)) assertApprox(wrenchRight, distribute.wrenchRight.value, 3) distribute.ankleWrenchLeft.recompute(0) distribute.ankleWrenchRight.recompute(0) print("resulting ankle left wrench: %s" % str(distribute.ankleWrenchLeft.value)) assertApprox(ankleWrenchLeft, distribute.ankleWrenchLeft.value, 3) print("resulting ankle right wrench: %s" % str(distribute.ankleWrenchRight.value)) assertApprox(ankleWrenchRight, distribute.ankleWrenchRight.value, 3)
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)
controller.vLeft.recompute(0) controller.gainRight.recompute(0) controller.gainLeft.recompute(0) # There is more pressure on the right foot. # Therefore, the right foot must go up to reduce it vRight = [0.] * 2 + [100.] + [0.] * 3 vLeft = [0.] * 2 + [-100.] + [0.] * 3 print("Expected vRight: %s" % str(vRight)) print("Actual vRight: %s" % str(controller.vRight.value)) print("Expected vLeft: %s" % str(vLeft)) print("Actual vLeft: %s" % str(controller.vLeft.value)) print() assertApprox(vRight, controller.vRight.value) assertApprox(vLeft, controller.vLeft.value) print("gainRight: %s" % str(controller.gainRight.value)) print("gainLeft: %s" % str(controller.gainLeft.value)) print() assertApprox(gainDouble, controller.gainRight.value) assertApprox(gainDouble, controller.gainLeft.value) print("---- Left support ----") controller.phase.value = 1 controller.vRight.recompute(1) controller.vLeft.recompute(1) controller.gainRight.recompute(1)
data2 = model.createData() pin.framesForwardKinematics(model, data2, q_est) print(data2.oMf[rightId]) print(data2.oMf[leftId]) # --- DCM estimator --- print("--- DCM estimator ---") dcm_estimator = DcmEstimator('dcm_estimator') dcm_estimator.init(dt, robot_name) plug(e2q.quaternion, dcm_estimator.q) plug(base_estimator.v, dcm_estimator.v) dcm_estimator.c.recompute(0) print(dcm_estimator.c.value) # --- Direct CoM --- print("--- Direct CoM ---") print(com.flatten().tolist()[0]) # --- Raw CoM difference --- print("--- Raw CoM difference ---") com_rawdiff = np.matrix(dcm_estimator.c.value).T - com print(com_rawdiff.flatten().tolist()[0]) # --- Relative CoM difference --- print("--- Relative CoM difference ---") com_reldiff = np.matrix(dcm_estimator.c.value).T - comRel print(com_reldiff.flatten().tolist()[0]) assertApprox(np.matrix(dcm_estimator.c.value).T, comRel, 3)
from sot_talos_balance.euler_to_quat import EulerToQuat from sot_talos_balance.pose_quaternion_to_matrix_homo import PoseQuaternionToMatrixHomo from sot_talos_balance.quat_to_euler import QuatToEuler # --- Euler to quat --- print("--- Euler to quat ---") signal_in = [0.0, 0.0, 0.5, 0.0, 0.0, np.pi, 0.2, 0.6] e2q = EulerToQuat('e2q') e2q.euler.value = signal_in print(e2q.euler.value) e2q.quaternion.recompute(0) print(e2q.quaternion.value) assertApprox(e2q.quaternion.value, [0.0, 0.0, 0.5, 0.0, 0.0, 1.0, 0.0, 0.2, 0.6], 6) # --- Quat to Euler --- print("--- Quat to Euler ---") signal_in = [0.0, 0.0, 0.5, 0.0, 0.0, 1.0, 0.0, 0.2, 0.6] q2e = QuatToEuler('q2e') q2e.quaternion.value = signal_in print(q2e.quaternion.value) q2e.euler.recompute(0) print(q2e.euler.value) assertApprox(q2e.euler.value, [0.0, 0.0, 0.5, 0.0, 0.0, np.pi, 0.2, 0.6], 6) # --- Quat to homogeneous --- print("--- Quat to homogeneous ---")
model = pin.buildModelFromUrdf(urdfPath, pin.JointModelFreeFlyer()) model.lowerPositionLimit = np.vstack((np.matrix([-1.] * 7).T, model.lowerPositionLimit[7:])) model.upperPositionLimit = np.vstack((np.matrix([-1.] * 7).T, model.upperPositionLimit[7:])) data = model.createData() q = pin.randomConfiguration(model) v = pin.utils.rand(model.nv) pin.centerOfMass(model, data, q, v) print("Expected:") print("CoM position value: {0}".format(tuple(data.com[0].flat))) print("CoM velocity value: {0}".format(tuple(data.vcom[0].flat))) conf.param_server = parameter_server_conf param_server = ParameterServer("param_server") param_server.init(dt, conf.param_server.urdfFileName, robot_name) param_server.setJointsUrdfToSot(conf.param_server.urdftosot) param_server.setRightFootForceSensorXYZ(conf.param_server.rightFootSensorXYZ) param_server.setRightFootSoleXYZ(conf.param_server.rightFootSoleXYZ) dcm_estimator = DcmEstimator('dcm_estimator') dcm_estimator.q.value = list(q.flat) dcm_estimator.v.value = list(v.flat) dcm_estimator.init(dt, robot_name) dcm_estimator.c.recompute(1) dcm_estimator.dc.recompute(1) print("Computed:") print("CoM position value: {0}".format(dcm_estimator.c.value)) assertApprox(data.com[0], np.matrix(dcm_estimator.c.value).T, 3) print("CoM velocity value: {0}".format(dcm_estimator.dc.value)) assertApprox(data.vcom[0], np.matrix(dcm_estimator.dc.value).T, 3)
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) print("Desired CoM velocity:") print(wp.vcomDes.value) assertApprox(vcomDes, wp.vcomDes.value, 3) print("Desired CoM acceleration:") print(wp.acomDes.value) assertApprox(acomDes, wp.acomDes.value, 3) print("Desired DCM:") print(wp.dcmDes.value) assertApprox(dcmDes, wp.dcmDes.value, 3) print("Desired ZMP:") print(wp.zmpDes.value) assertApprox(zmpDes, wp.zmpDes.value, 3)
import eigenpy eigenpy.switchToNumpyMatrix() dt = 1e-3 initstate = [0.] * 9 integrator = SimpleStateIntegrator("integrator") integrator.init(dt) integrator.setState(initstate) integrator.setVelocity([0.] * 9) t = 0 integrator.control.value = [0.] * 9 integrator.state.recompute(0) assertApprox(integrator.state.value, initstate) t += 1 integrator.control.value = [1., 0., 0.] + [0.] * 6 integrator.state.recompute(t) assertApprox(integrator.state.value, [dt, 0., 0.] + [0.] * 6) t += 1 integrator.control.value = [0., 1., 0.] + [0.] * 6 integrator.state.recompute(t) assertApprox(integrator.state.value, [dt, dt, 0.] + [0.] * 6) t += 1 integrator.control.value = [0., 0., 1.] + [0.] * 6 integrator.state.recompute(t) assertApprox(integrator.state.value, [dt, dt, dt] + [0.] * 6)
controller.Kp.value = Kp controller.state.value = state controller.tauDes.value = tauDes controller.tau.value = tau print("\nKp: %s" % (controller.Kp.value, )) print("\nq: %s" % (controller.state.value, )) print("tauDes: %s" % (controller.tauDes.value, )) print("tau: %s" % (controller.tau.value, )) q = tuple(N_JOINTS * [1.0]) dt = 1 controller.init(dt, N_JOINTS) controller.setPosition(q) controller.qRef.recompute(0) print("\nqRef: %s" % (controller.qRef.value, )) assertApprox(controller.qRef.value, q) tauDes = tuple(N_JOINTS * [1.0]) controller.tauDes.value = tauDes controller.qRef.recompute(1) print("\nqRef: %s" % (controller.qRef.value, )) qRef = tuple([q[i] + Kp[i] * (tauDes[i] - tau[i]) * dt for i in range(len(q))]) assertApprox(controller.qRef.value, qRef)