def talker(): compDyn = ComputationalDynamics() p = HyQSim() p.start() p.register_node() name = "Support region" params = IterativeProjectionParameters() foothold_params = FootholdPlanningInterface() i = 0 p.get_sim_wbs() params.getParamsFromRosDebugTopic(p.hyq_rcf_debug) foothold_params.getParamsFromRosDebugTopic(p.hyq_rcf_debug) params.getCurrentStanceFeetFlags(p.hyq_rcf_debug) params.getCurrentFeetPos(p.hyq_rcf_debug) """ contact points """ ng = 4 params.setNumberOfFrictionConesEdges(ng) while not ros.is_shutdown(): p.get_sim_wbs() params.getParamsFromRosDebugTopic(p.hyq_rcf_debug) foothold_params.getParamsFromRosDebugTopic(p.hyq_rcf_debug) params.getCurrentStanceFeetFlags(p.hyq_rcf_debug) # params.getCurrentFeetPosFlags(p.hyq_rcf_debug) # ONLY_ACTUATION, ONLY_FRICTION or FRICTION_AND_ACTUATION # 3 - FRICTION REGION constraint_mode_IP = 'ONLY_FRICTION' params.setConstraintModes([ constraint_mode_IP, constraint_mode_IP, constraint_mode_IP, constraint_mode_IP ]) params.setNumberOfFrictionConesEdges(ng) frictionRegionWF, actuation_polygons, computation_time = compDyn.try_iterative_projection_bretl( params) print frictionRegionWF p.send_support_region(name, p.fillPolygon(frictionRegionWF)) time.sleep(0.05) i += 1 print 'de registering...' p.deregister_node()
def talker(): compDyn = ComputationalDynamics() math = Math() p = HyQSim() p.start() p.register_node() name = "Actuation_region" point = Point() actuationParams = ActuationParameters() i = 0 start_t_IP = time.time() for j in range(0, 1000): vertices = [point] # print("Time: " + str(i*0.004) + "s and Simulation time: " + str(p.get_sim_time()/60)) p.get_sim_wbs() actuationParams.getParams(p.hyq_rcf_debug) trunk_mass = 85. axisZ = np.array([[0.0], [0.0], [1.0]]) ''' normals ''' n1 = np.transpose( np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n2 = np.transpose( np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n3 = np.transpose( np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n4 = np.transpose( np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) normals = np.vstack([n1, n2, n3]) """ contact points """ nc = actuationParams.numberOfContacts contacts = actuationParams.contacts[0:nc + 1, :] print 'contacts number: ', actuationParams.numberOfContacts print contacts, actuationParams.stanceFeet IAR, actuation_polygons, computation_time = compDyn.instantaneous_actuation_region_bretl( actuationParams.stanceFeet, contacts, normals, trunk_mass) number_of_vertices = np.size(IAR, 0) # number_of_vertices = 10 # print IAR for i in range(0, number_of_vertices): point = Point() point.x = IAR[i][0] point.y = IAR[i][1] point.z = 0.0 vertices = np.hstack([vertices, point]) # print'vertices', vertices p.send_polygons(name, vertices) time.sleep(1.0 / 5.0) i += 1 print 'de registering...' p.deregister_node() computation_time = (time.time() - start_t_IP) print("Total time: --- %s seconds ---" % computation_time) print 'number of published messages ', actuationParams.numberOfPublishedMessages avgTime = computation_time / actuationParams.numberOfPublishedMessages print 'average publishing time [ms]', avgTime print 'average publishing frequency [Hz]', 1.0 / avgTime print 'number of received messages ', p.numberOfReceivedMessages avgTime = computation_time / p.numberOfReceivedMessages print 'average subscription time [ms]', avgTime print 'average subscription frequency [Hz]', 1.0 / avgTime
n1 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n2 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n3 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n4 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) normals = np.vstack([n1, n2, n3, n4]) LF_tau_lim = [50.0, 100.0, 100.0] RF_tau_lim = [50.0, 100.0, 100.0] LH_tau_lim = [50.0, 100.0, 100.0] RH_tau_lim = [50.0, 100.0, 100.0] torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) nc = np.sum(stanceFeet) comp_dyn = ComputationalDynamics() params = IterativeProjectionParameters() params.setContactsPosWF(contacts) params.setTorqueLims(torque_limits) params.setActiveContacts(stanceFeet) params.setConstraintModes(constraint_mode_IP) params.setContactNormals(normals) params.setFrictionCoefficient(mu) params.setNumberOfFrictionConesEdges(ng) params.setTotalMass(trunk_mass) #inputs for foothold planning foothold_params = FootholdPlanningInterface() foothold_params.com_position_to_validateW = [0.05, 0.05, 0.55] maxCorrection = 0.2
from numpy import array, cross, dot, eye, hstack, vstack, zeros import matplotlib.pyplot as plt from jet_leg.plotting_tools import Plotter from jet_leg.constraints import Constraints from jet_leg.hyq_kinematics import HyQKinematics from jet_leg.math_tools import Math from jet_leg.computational_dynamics import ComputationalDynamics from jet_leg.height_map import HeightMap from path_sequential_iterative_projection import PathIterativeProjection ''' MAIN ''' start_t_IPVC = time.time() math = Math() compDyn = ComputationalDynamics() pathIP = PathIterativeProjection() # number of contacts nc = 3 # number of generators, i.e. rays used to linearize the friction cone ng = 4 # ONLY_ACTUATION or ONLY_FRICTION constraint_mode = 'ONLY_ACTUATION' useVariableJacobian = True trunk_mass = 100 mu = 0.8 # number of decision variables of the problem n = nc * 6 i = 0 comTrajectoriesToStack = np.zeros((0, 3))
from scipy.linalg import block_diag from numpy import array, cross, dot, eye, hstack, vstack, zeros, matrix import matplotlib.pyplot as plt from jet_leg.plotting_tools import Plotter from jet_leg.constraints import Constraints from jet_leg.hyq_kinematics import HyQKinematics from jet_leg.math_tools import Math from jet_leg.computational_dynamics import ComputationalDynamics from jet_leg.vertex_based_projection import VertexBasedProjection from jet_leg.iterative_projection_parameters import IterativeProjectionParameters ''' MAIN ''' start_t_IPVC = time.time() math = Math() compDyn = ComputationalDynamics() # number of generators, i.e. rays used to linearize the friction cone ng = 4 # ONLY_ACTUATION or ONLY_FRICTION or FRICTION_AND_ACTUATION constraint_mode_IP = [ 'ONLY_ACTUATION', 'ONLY_ACTUATION', 'ONLY_ACTUATION', 'ONLY_ACTUATION' ] useVariableJacobian = True g = 9.81 trunk_mass = 85. mu = 0.9 """ contact points """ LF_foot = np.array([0.3, 0.2, -0.5]) RF_foot = np.array([0.3, -0.2, -0.5]) LH_foot = np.array([-0.3, 0.2, -0.5])
def test_LP_friction_constraints_only(self): math = Math() # number of contacts nc = 3 # number of generators, i.e. rays used to linearize the friction cone ng = 4 # ONLY_FRICTION constraint_mode_IP = ['ONLY_FRICTION', 'ONLY_FRICTION', 'ONLY_FRICTION', 'ONLY_FRICTION'] useVariableJacobian = True LF_foot = np.array([0.3, 0.2, -0.65]) RF_foot = np.array([0.3, -0.2, -0.65]) LH_foot = np.array([-0.2, 0.2, -0.4]) RH_foot = np.array([-0.3, -0.2, -0.65]) contactsToStack = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) contacts = contactsToStack[0:4, :] ''' parameters to be tuned''' g = 9.81 trunk_mass = 90. mu = 0.8 axisZ= np.array([[0.0], [0.0], [1.0]]) n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) # %% Cell 2 ''' stanceFeet vector contains 1 is the foot is on the ground and 0 if it is in the air''' stanceFeet = [1, 1, 1, 1] randomSwingLeg = random.randint(0, 3) tripleStance = True # if you want you can define a swing leg using this variable if tripleStance: print 'Swing leg', randomSwingLeg stanceFeet[randomSwingLeg] = 0 print 'stanceLegs ', stanceFeet normals = np.vstack([n1, n2, n3, n4]) comp_dyn = ComputationalDynamics() params = IterativeProjectionParameters() params.setContactsPosWF(contacts) #params.setCoMPosWF(comWF) # params.setTorqueLims(torque_limits) params.setActiveContacts(stanceFeet) params.setConstraintModes(constraint_mode_IP) params.setContactNormals(normals) params.setFrictionCoefficient(mu) params.setNumberOfFrictionConesEdges(ng) params.setTotalMass(trunk_mass) feasible, unfeasible, contact_forces = comp_dyn.LP_projection(params) print 'result',feasible, unfeasible, contact_forces expected_feasible = np.array([[ -1.00000000e-01, 1.50000000e-01, -2.00000000e-01], [ -1.00000000e-01, 1.50000000e-01, -1.50000000e-01], [ -1.00000000e-01, 1.50000000e-01, -1.00000000e-01], [ -1.00000000e-01, 1.50000000e-01, -5.00000000e-02], [ -1.00000000e-01, 1.50000000e-01, -5.55111512e-17], [ -1.00000000e-01, 1.50000000e-01, 5.00000000e-02], [ -1.00000000e-01, 1.50000000e-01, 1.00000000e-01], [ -5.00000000e-02, 1.00000000e-01, -2.00000000e-01], [ -5.00000000e-02, 1.00000000e-01, -1.50000000e-01], [ -5.00000000e-02, 1.00000000e-01, -1.00000000e-01], [ -5.00000000e-02, 1.00000000e-01, -5.00000000e-02], [ -5.00000000e-02, 1.00000000e-01, -5.55111512e-17], [ -5.00000000e-02, 1.00000000e-01, 5.00000000e-02], [ -5.00000000e-02, 1.00000000e-01, 1.00000000e-01], [ -5.00000000e-02, 1.50000000e-01, -2.00000000e-01], [ -5.00000000e-02, 1.50000000e-01, -1.50000000e-01], [ -5.00000000e-02, 1.50000000e-01, -1.00000000e-01], [ -5.00000000e-02, 1.50000000e-01, -5.00000000e-02], [ -5.00000000e-02, 1.50000000e-01, -5.55111512e-17], [ -5.00000000e-02, 1.50000000e-01, 5.00000000e-02], [ -5.00000000e-02, 1.50000000e-01, 1.00000000e-01], [ -1.11022302e-16, 5.00000000e-02, -2.00000000e-01], [ -1.11022302e-16, 5.00000000e-02, -1.50000000e-01], [ -1.11022302e-16, 5.00000000e-02, -1.00000000e-01], [ -1.11022302e-16, 5.00000000e-02, -5.00000000e-02], [ -1.11022302e-16, 5.00000000e-02, -5.55111512e-17], [ -1.11022302e-16, 5.00000000e-02, 5.00000000e-02], [ -1.11022302e-16, 5.00000000e-02, 1.00000000e-01], [ -1.11022302e-16, 1.00000000e-01, -2.00000000e-01], [ -1.11022302e-16, 1.00000000e-01, -1.50000000e-01], [ -1.11022302e-16, 1.00000000e-01, -1.00000000e-01], [ -1.11022302e-16, 1.00000000e-01, -5.00000000e-02], [ -1.11022302e-16, 1.00000000e-01, -5.55111512e-17], [ -1.11022302e-16, 1.00000000e-01, 5.00000000e-02], [ -1.11022302e-16, 1.00000000e-01, 1.00000000e-01], [ -1.11022302e-16, 1.50000000e-01, -2.00000000e-01], [ -1.11022302e-16, 1.50000000e-01, -1.50000000e-01], [ -1.11022302e-16, 1.50000000e-01, -1.00000000e-01], [ -1.11022302e-16, 1.50000000e-01, -5.00000000e-02], [ -1.11022302e-16, 1.50000000e-01, -5.55111512e-17], [ -1.11022302e-16, 1.50000000e-01, 5.00000000e-02], [ -1.11022302e-16, 1.50000000e-01, 1.00000000e-01], [ 5.00000000e-02, -1.11022302e-16, -2.00000000e-01], [ 5.00000000e-02, -1.11022302e-16, -1.50000000e-01], [ 5.00000000e-02, -1.11022302e-16, -1.00000000e-01], [ 5.00000000e-02, -1.11022302e-16, -5.00000000e-02], [ 5.00000000e-02, -1.11022302e-16, -5.55111512e-17], [ 5.00000000e-02, -1.11022302e-16, 5.00000000e-02], [ 5.00000000e-02, -1.11022302e-16, 1.00000000e-01], [ 5.00000000e-02, 5.00000000e-02, -2.00000000e-01], [ 5.00000000e-02, 5.00000000e-02, -1.50000000e-01], [ 5.00000000e-02, 5.00000000e-02, -1.00000000e-01], [ 5.00000000e-02, 5.00000000e-02, -5.00000000e-02], [ 5.00000000e-02, 5.00000000e-02, -5.55111512e-17], [ 5.00000000e-02, 5.00000000e-02, 5.00000000e-02], [ 5.00000000e-02, 5.00000000e-02, 1.00000000e-01], [ 5.00000000e-02, 1.00000000e-01, -2.00000000e-01], [ 5.00000000e-02, 1.00000000e-01, -1.50000000e-01], [ 5.00000000e-02, 1.00000000e-01, -1.00000000e-01], [ 5.00000000e-02, 1.00000000e-01, -5.00000000e-02], [ 5.00000000e-02, 1.00000000e-01, -5.55111512e-17], [ 5.00000000e-02, 1.00000000e-01, 5.00000000e-02], [ 5.00000000e-02, 1.00000000e-01, 1.00000000e-01], [ 5.00000000e-02, 1.50000000e-01, -2.00000000e-01], [ 5.00000000e-02, 1.50000000e-01, -1.50000000e-01], [ 5.00000000e-02, 1.50000000e-01, -1.00000000e-01], [ 5.00000000e-02, 1.50000000e-01, -5.00000000e-02], [ 5.00000000e-02, 1.50000000e-01, -5.55111512e-17], [ 5.00000000e-02, 1.50000000e-01, 5.00000000e-02], [ 5.00000000e-02, 1.50000000e-01, 1.00000000e-01], [ 5.00000000e-02, 2.00000000e-01, -2.00000000e-01], [ 5.00000000e-02, 2.00000000e-01, -1.50000000e-01], [ 5.00000000e-02, 2.00000000e-01, -1.00000000e-01], [ 5.00000000e-02, 2.00000000e-01, -5.00000000e-02], [ 5.00000000e-02, 2.00000000e-01, -5.55111512e-17], [ 5.00000000e-02, 2.00000000e-01, 5.00000000e-02], [ 5.00000000e-02, 2.00000000e-01, 1.00000000e-01], [ 1.00000000e-01, -1.11022302e-16, -2.00000000e-01], [ 1.00000000e-01, -1.11022302e-16, -1.50000000e-01], [ 1.00000000e-01, -1.11022302e-16, -1.00000000e-01], [ 1.00000000e-01, -1.11022302e-16, -5.00000000e-02], [ 1.00000000e-01, -1.11022302e-16, -5.55111512e-17], [ 1.00000000e-01, -1.11022302e-16, 5.00000000e-02], [ 1.00000000e-01, -1.11022302e-16, 1.00000000e-01], [ 1.00000000e-01, 5.00000000e-02, -2.00000000e-01], [ 1.00000000e-01, 5.00000000e-02, -1.50000000e-01], [ 1.00000000e-01, 5.00000000e-02, -1.00000000e-01], [ 1.00000000e-01, 5.00000000e-02, -5.00000000e-02], [ 1.00000000e-01, 5.00000000e-02, -5.55111512e-17], [ 1.00000000e-01, 5.00000000e-02, 5.00000000e-02], [ 1.00000000e-01, 5.00000000e-02, 1.00000000e-01], [ 1.00000000e-01, 1.00000000e-01, -2.00000000e-01], [ 1.00000000e-01, 1.00000000e-01, -1.50000000e-01], [ 1.00000000e-01, 1.00000000e-01, -1.00000000e-01], [ 1.00000000e-01, 1.00000000e-01, -5.00000000e-02], [ 1.00000000e-01, 1.00000000e-01, -5.55111512e-17], [ 1.00000000e-01, 1.00000000e-01, 5.00000000e-02], [ 1.00000000e-01, 1.00000000e-01, 1.00000000e-01], [ 1.00000000e-01, 1.50000000e-01, -2.00000000e-01], [ 1.00000000e-01, 1.50000000e-01, -1.50000000e-01], [ 1.00000000e-01, 1.50000000e-01, -1.00000000e-01], [ 1.00000000e-01, 1.50000000e-01, -5.00000000e-02], [ 1.00000000e-01, 1.50000000e-01, -5.55111512e-17], [ 1.00000000e-01, 1.50000000e-01, 5.00000000e-02], [ 1.00000000e-01, 1.50000000e-01, 1.00000000e-01], [ 1.50000000e-01, -5.00000000e-02, -2.00000000e-01], [ 1.50000000e-01, -5.00000000e-02, -1.50000000e-01], [ 1.50000000e-01, -5.00000000e-02, -1.00000000e-01], [ 1.50000000e-01, -5.00000000e-02, -5.00000000e-02], [ 1.50000000e-01, -5.00000000e-02, -5.55111512e-17], [ 1.50000000e-01, -5.00000000e-02, 5.00000000e-02], [ 1.50000000e-01, -1.11022302e-16, -2.00000000e-01], [ 1.50000000e-01, -1.11022302e-16, -1.50000000e-01], [ 1.50000000e-01, -1.11022302e-16, -1.00000000e-01], [ 1.50000000e-01, -1.11022302e-16, -5.00000000e-02], [ 1.50000000e-01, -1.11022302e-16, -5.55111512e-17], [ 1.50000000e-01, -1.11022302e-16, 5.00000000e-02], [ 1.50000000e-01, 5.00000000e-02, -2.00000000e-01], [ 1.50000000e-01, 5.00000000e-02, -1.50000000e-01], [ 1.50000000e-01, 5.00000000e-02, -1.00000000e-01], [ 1.50000000e-01, 5.00000000e-02, -5.00000000e-02], [ 1.50000000e-01, 5.00000000e-02, -5.55111512e-17], [ 1.50000000e-01, 5.00000000e-02, 5.00000000e-02], [ 1.50000000e-01, 1.00000000e-01, -2.00000000e-01], [ 1.50000000e-01, 1.00000000e-01, -1.50000000e-01], [ 1.50000000e-01, 1.00000000e-01, -1.00000000e-01], [ 1.50000000e-01, 1.00000000e-01, -5.00000000e-02], [ 1.50000000e-01, 1.00000000e-01, -5.55111512e-17], [ 1.50000000e-01, 1.00000000e-01, 5.00000000e-02], [ 1.50000000e-01, 1.50000000e-01, -2.00000000e-01], [ 1.50000000e-01, 1.50000000e-01, -1.50000000e-01], [ 1.50000000e-01, 1.50000000e-01, -1.00000000e-01], [ 1.50000000e-01, 1.50000000e-01, -5.00000000e-02], [ 1.50000000e-01, 1.50000000e-01, -5.55111512e-17], [ 1.50000000e-01, 1.50000000e-01, 5.00000000e-02], [ 2.00000000e-01, -1.00000000e-01, -2.00000000e-01], [ 2.00000000e-01, -1.00000000e-01, -1.50000000e-01], [ 2.00000000e-01, -1.00000000e-01, -1.00000000e-01], [ 2.00000000e-01, -1.00000000e-01, -5.00000000e-02], [ 2.00000000e-01, -1.00000000e-01, -5.55111512e-17], [ 2.00000000e-01, -1.00000000e-01, 5.00000000e-02], [ 2.00000000e-01, -5.00000000e-02, -2.00000000e-01], [ 2.00000000e-01, -5.00000000e-02, -1.50000000e-01], [ 2.00000000e-01, -5.00000000e-02, -1.00000000e-01], [ 2.00000000e-01, -5.00000000e-02, -5.00000000e-02], [ 2.00000000e-01, -5.00000000e-02, -5.55111512e-17], [ 2.00000000e-01, -5.00000000e-02, 5.00000000e-02], [ 2.00000000e-01, -1.11022302e-16, -2.00000000e-01], [ 2.00000000e-01, -1.11022302e-16, -1.50000000e-01], [ 2.00000000e-01, -1.11022302e-16, -1.00000000e-01], [ 2.00000000e-01, -1.11022302e-16, -5.00000000e-02], [ 2.00000000e-01, -1.11022302e-16, -5.55111512e-17], [ 2.00000000e-01, -1.11022302e-16, 5.00000000e-02], [ 2.00000000e-01, 5.00000000e-02, -2.00000000e-01], [ 2.00000000e-01, 5.00000000e-02, -1.50000000e-01], [ 2.00000000e-01, 5.00000000e-02, -1.00000000e-01], [ 2.00000000e-01, 5.00000000e-02, -5.00000000e-02], [ 2.00000000e-01, 5.00000000e-02, -5.55111512e-17], [ 2.00000000e-01, 5.00000000e-02, 5.00000000e-02], [ 2.00000000e-01, 1.00000000e-01, -2.00000000e-01], [ 2.00000000e-01, 1.00000000e-01, -1.50000000e-01], [ 2.00000000e-01, 1.00000000e-01, -1.00000000e-01], [ 2.00000000e-01, 1.00000000e-01, -5.00000000e-02], [ 2.00000000e-01, 1.00000000e-01, -5.55111512e-17], [ 2.00000000e-01, 1.00000000e-01, 5.00000000e-02], [ 2.00000000e-01, 1.50000000e-01, -2.00000000e-01], [ 2.00000000e-01, 1.50000000e-01, -1.50000000e-01], [ 2.00000000e-01, 1.50000000e-01, -1.00000000e-01], [ 2.00000000e-01, 1.50000000e-01, -5.00000000e-02], [ 2.00000000e-01, 1.50000000e-01, -5.55111512e-17], [ 2.00000000e-01, 1.50000000e-01, 5.00000000e-02], [ 2.00000000e-01, 2.00000000e-01, -2.00000000e-01], [ 2.00000000e-01, 2.00000000e-01, -1.50000000e-01], [ 2.00000000e-01, 2.00000000e-01, -1.00000000e-01], [ 2.00000000e-01, 2.00000000e-01, -5.00000000e-02], [ 2.00000000e-01, 2.00000000e-01, -5.55111512e-17], [ 2.00000000e-01, 2.00000000e-01, 5.00000000e-02], [ 2.50000000e-01, -1.50000000e-01, -2.00000000e-01], [ 2.50000000e-01, -1.50000000e-01, -1.50000000e-01], [ 2.50000000e-01, -1.50000000e-01, -1.00000000e-01], [ 2.50000000e-01, -1.50000000e-01, -5.00000000e-02], [ 2.50000000e-01, -1.50000000e-01, -5.55111512e-17], [ 2.50000000e-01, -1.00000000e-01, -2.00000000e-01], [ 2.50000000e-01, -1.00000000e-01, -1.50000000e-01], [ 2.50000000e-01, -1.00000000e-01, -1.00000000e-01], [ 2.50000000e-01, -1.00000000e-01, -5.00000000e-02], [ 2.50000000e-01, -1.00000000e-01, -5.55111512e-17], [ 2.50000000e-01, -5.00000000e-02, -2.00000000e-01], [ 2.50000000e-01, -5.00000000e-02, -1.50000000e-01], [ 2.50000000e-01, -5.00000000e-02, -1.00000000e-01], [ 2.50000000e-01, -5.00000000e-02, -5.00000000e-02], [ 2.50000000e-01, -5.00000000e-02, -5.55111512e-17], [ 2.50000000e-01, -1.11022302e-16, -2.00000000e-01], [ 2.50000000e-01, -1.11022302e-16, -1.50000000e-01], [ 2.50000000e-01, -1.11022302e-16, -1.00000000e-01], [ 2.50000000e-01, -1.11022302e-16, -5.00000000e-02], [ 2.50000000e-01, -1.11022302e-16, -5.55111512e-17], [ 2.50000000e-01, 5.00000000e-02, -2.00000000e-01], [ 2.50000000e-01, 5.00000000e-02, -1.50000000e-01], [ 2.50000000e-01, 5.00000000e-02, -1.00000000e-01], [ 2.50000000e-01, 5.00000000e-02, -5.00000000e-02], [ 2.50000000e-01, 5.00000000e-02, -5.55111512e-17], [ 2.50000000e-01, 1.00000000e-01, -2.00000000e-01], [ 2.50000000e-01, 1.00000000e-01, -1.50000000e-01], [ 2.50000000e-01, 1.00000000e-01, -1.00000000e-01], [ 2.50000000e-01, 1.00000000e-01, -5.00000000e-02], [ 2.50000000e-01, 1.00000000e-01, -5.55111512e-17], [ 2.50000000e-01, 1.50000000e-01, -2.00000000e-01], [ 2.50000000e-01, 1.50000000e-01, -1.50000000e-01], [ 2.50000000e-01, 1.50000000e-01, -1.00000000e-01], [ 2.50000000e-01, 1.50000000e-01, -5.00000000e-02], [ 2.50000000e-01, 1.50000000e-01, -5.55111512e-17], [ 3.00000000e-01, -1.50000000e-01, -2.00000000e-01], [ 3.00000000e-01, -1.50000000e-01, -1.50000000e-01], [ 3.00000000e-01, -1.50000000e-01, -1.00000000e-01], [ 3.00000000e-01, -1.50000000e-01, -5.00000000e-02], [ 3.00000000e-01, -1.50000000e-01, -5.55111512e-17], [ 3.00000000e-01, 1.50000000e-01, -2.00000000e-01], [ 3.00000000e-01, 1.50000000e-01, -1.50000000e-01], [ 3.00000000e-01, 1.50000000e-01, -1.00000000e-01], [ 3.00000000e-01, 1.50000000e-01, -5.00000000e-02], [ 3.00000000e-01, 1.50000000e-01, -5.55111512e-17]]); # print expected_feasible rows = np.size(expected_feasible, 0) cols = np.size(expected_feasible, 1) for i in range(0, rows): for j in range (0, cols): self.assertAlmostEquals(expected_feasible[i,j], feasible[i,j], self.assertPrecision)
def test_lp_stability_check(self): math = Math() # number of contacts nc = 3 # number of generators, i.e. rays used to linearize the friction cone ng = 4 # ONLY_FRICTION # ONLY_ACTUATION constraint_mode_IP = ['ONLY_ACTUATION', 'ONLY_ACTUATION', 'ONLY_ACTUATION', 'ONLY_ACTUATION'] useVariableJacobian = True comWF = np.array([1.25, 0.0, 0.0]) """ contact points in the World Frame""" LF_foot = np.array([1.3, 0.2, -0.6]) RF_foot = np.array([1.3, -0.2, -0.5]) LH_foot = np.array([0.7, 0.2, -0.45]) RH_foot = np.array([0.7, -0.2, -0.5]) contactsToStack = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot)) contacts = contactsToStack[0:4, :] ''' parameters to be tuned''' g = 9.81 trunk_mass = 50. mu = 0.8 axisZ = np.array([[0.0], [0.0], [1.0]]) n1 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n2 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n3 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n4 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # %% Cell 2 ''' stanceFeet vector contains 1 is the foot is on the ground and 0 if it is in the air''' stanceFeet = [1, 1, 1, 1] randomSwingLeg = random.randint(0, 3) tripleStance = True # if you want you can define a swing leg using this variable if tripleStance: print 'Swing leg', randomSwingLeg #stanceFeet[randomSwingLeg] = 0 print 'stanceLegs ', stanceFeet normals = np.vstack([n1, n2, n3, n4]) comp_dyn = ComputationalDynamics() params = IterativeProjectionParameters() params.setContactsPosWF(contacts) params.setCoMPosWF(comWF) # params.setTorqueLims(torque_limits) params.setActiveContacts(stanceFeet) params.setConstraintModes(constraint_mode_IP) params.setContactNormals(normals) params.setFrictionCoefficient(mu) params.setNumberOfFrictionConesEdges(ng) params.setTotalMass(trunk_mass) status, x, force_polytopes = comp_dyn.check_equilibrium(params) print status, x '''Plotting the contact points in the 3D figure''' fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.set_xlabel('X axis') ax.set_ylabel('Y axis') ax.set_zlabel('Z axis') ''' plotting Iterative Projection points ''' plotter = Plotter() scaling_factor = 2000 stanceID = params.getStanceIndex(stanceFeet) for j in range(0, nc): # this will only show the force polytopes of the feet that are defined to be in stance idx = int(stanceID[j]) if (constraint_mode_IP[idx] == 'ONLY_ACTUATION') or (constraint_mode_IP[idx] == 'FRICTION_AND_ACTUATION'): plotter.plot_actuation_polygon(ax, force_polytopes[idx], contacts[idx, :], scaling_factor) plt.show()
def test_LP_actuation_constraints_only(self): math = Math() # number of contacts nc = 3 # number of generators, i.e. rays used to linearize the friction cone ng = 4 # ONLY_ACTUATION or ONLY_FRICTION constraint_mode = 'ONLY_ACTUATION' useVariableJacobian = True """ contact points """ LF_foot = np.array([0.3, 0.2, -0.65]) RF_foot = np.array([0.3, -0.2, -0.65]) LH_foot = np.array([-0.2, 0.2, -0.4]) RH_foot = np.array([-0.3, -0.2, -0.65]) contactsToStack = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) contacts = contactsToStack[0:nc, :] ''' parameters to be tuned''' g = 9.81 trunk_mass = 90. mu = 0.8 axisZ= np.array([[0.0], [0.0], [1.0]]) n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) # %% Cell 2 normals = np.vstack([n1, n2, n3, n4]) comp_dyn = ComputationalDynamics() params = IterativeProjectionParameters() params.setContactsPosWF(contacts) # params.setCoMPosWF(comWF) # params.setTorqueLims(torque_limits) # params.setActiveContacts(stanceFeet) # params.setConstraintModes(constraint_mode_IP) params.setContactNormals(normals) params.setFrictionCoefficient(mu) params.setNumberOfFrictionConesEdges(ng) params.setTotalMass(trunk_mass) feasible, unfeasible, contact_forces = comp_dyn.LP_projection(params) expected_feasible = np.array([[ -1.50000000e-01, 2.50000000e-01, 5.00000000e-02], [ -1.50000000e-01, 2.50000000e-01, 1.00000000e-01], [ -1.50000000e-01, 3.00000000e-01, 5.00000000e-02], [ -1.50000000e-01, 3.00000000e-01, 1.00000000e-01], [ -1.50000000e-01, 3.50000000e-01, 1.00000000e-01], [ -1.00000000e-01, 1.00000000e-01, -5.55111512e-17], [ -1.00000000e-01, 1.00000000e-01, 5.00000000e-02], [ -1.00000000e-01, 1.00000000e-01, 1.00000000e-01], [ -1.00000000e-01, 1.50000000e-01, -5.55111512e-17], [ -1.00000000e-01, 1.50000000e-01, 5.00000000e-02], [ -1.00000000e-01, 1.50000000e-01, 1.00000000e-01], [ -1.00000000e-01, 2.00000000e-01, -5.55111512e-17], [ -1.00000000e-01, 2.00000000e-01, 5.00000000e-02], [ -1.00000000e-01, 2.00000000e-01, 1.00000000e-01], [ -1.00000000e-01, 2.50000000e-01, -5.55111512e-17], [ -1.00000000e-01, 2.50000000e-01, 5.00000000e-02], [ -1.00000000e-01, 2.50000000e-01, 1.00000000e-01], [ -1.00000000e-01, 3.00000000e-01, -5.55111512e-17], [ -1.00000000e-01, 3.00000000e-01, 5.00000000e-02], [ -1.00000000e-01, 3.00000000e-01, 1.00000000e-01], [ -1.00000000e-01, 3.50000000e-01, 5.00000000e-02], [ -1.00000000e-01, 3.50000000e-01, 1.00000000e-01], [ -5.00000000e-02, -1.11022302e-16, -1.00000000e-01], [ -5.00000000e-02, -1.11022302e-16, -5.00000000e-02], [ -5.00000000e-02, -1.11022302e-16, -5.55111512e-17], [ -5.00000000e-02, -1.11022302e-16, 5.00000000e-02], [ -5.00000000e-02, -1.11022302e-16, 1.00000000e-01], [ -5.00000000e-02, 5.00000000e-02, -1.50000000e-01], [ -5.00000000e-02, 5.00000000e-02, -1.00000000e-01], [ -5.00000000e-02, 5.00000000e-02, -5.00000000e-02], [ -5.00000000e-02, 5.00000000e-02, -5.55111512e-17], [ -5.00000000e-02, 5.00000000e-02, 5.00000000e-02], [ -5.00000000e-02, 5.00000000e-02, 1.00000000e-01], [ -5.00000000e-02, 1.00000000e-01, -2.00000000e-01], [ -5.00000000e-02, 1.00000000e-01, -1.50000000e-01], [ -5.00000000e-02, 1.00000000e-01, -1.00000000e-01], [ -5.00000000e-02, 1.00000000e-01, -5.00000000e-02], [ -5.00000000e-02, 1.00000000e-01, -5.55111512e-17], [ -5.00000000e-02, 1.00000000e-01, 5.00000000e-02], [ -5.00000000e-02, 1.00000000e-01, 1.00000000e-01], [ -5.00000000e-02, 1.50000000e-01, -2.00000000e-01], [ -5.00000000e-02, 1.50000000e-01, -1.50000000e-01], [ -5.00000000e-02, 1.50000000e-01, -1.00000000e-01], [ -5.00000000e-02, 1.50000000e-01, -5.00000000e-02], [ -5.00000000e-02, 1.50000000e-01, -5.55111512e-17], [ -5.00000000e-02, 1.50000000e-01, 5.00000000e-02], [ -5.00000000e-02, 1.50000000e-01, 1.00000000e-01], [ -5.00000000e-02, 2.00000000e-01, -2.00000000e-01], [ -5.00000000e-02, 2.00000000e-01, -1.50000000e-01], [ -5.00000000e-02, 2.00000000e-01, -1.00000000e-01], [ -5.00000000e-02, 2.00000000e-01, -5.00000000e-02], [ -5.00000000e-02, 2.00000000e-01, -5.55111512e-17], [ -5.00000000e-02, 2.00000000e-01, 5.00000000e-02], [ -5.00000000e-02, 2.00000000e-01, 1.00000000e-01], [ -5.00000000e-02, 2.50000000e-01, -2.00000000e-01], [ -5.00000000e-02, 2.50000000e-01, -1.50000000e-01], [ -5.00000000e-02, 2.50000000e-01, -1.00000000e-01], [ -5.00000000e-02, 2.50000000e-01, -5.00000000e-02], [ -5.00000000e-02, 2.50000000e-01, -5.55111512e-17], [ -5.00000000e-02, 2.50000000e-01, 5.00000000e-02], [ -5.00000000e-02, 2.50000000e-01, 1.00000000e-01], [ -5.00000000e-02, 3.00000000e-01, -5.00000000e-02], [ -5.00000000e-02, 3.00000000e-01, -5.55111512e-17], [ -5.00000000e-02, 3.00000000e-01, 5.00000000e-02], [ -5.00000000e-02, 3.00000000e-01, 1.00000000e-01], [ -5.00000000e-02, 3.50000000e-01, -5.55111512e-17], [ -5.00000000e-02, 3.50000000e-01, 5.00000000e-02], [ -5.00000000e-02, 3.50000000e-01, 1.00000000e-01], [ -1.11022302e-16, -5.00000000e-02, -1.00000000e-01], [ -1.11022302e-16, -5.00000000e-02, -5.00000000e-02], [ -1.11022302e-16, -5.00000000e-02, -5.55111512e-17], [ -1.11022302e-16, -5.00000000e-02, 5.00000000e-02], [ -1.11022302e-16, -5.00000000e-02, 1.00000000e-01], [ -1.11022302e-16, -1.11022302e-16, -1.50000000e-01], [ -1.11022302e-16, -1.11022302e-16, -1.00000000e-01], [ -1.11022302e-16, -1.11022302e-16, -5.00000000e-02], [ -1.11022302e-16, -1.11022302e-16, -5.55111512e-17], [ -1.11022302e-16, -1.11022302e-16, 5.00000000e-02], [ -1.11022302e-16, -1.11022302e-16, 1.00000000e-01], [ -1.11022302e-16, 5.00000000e-02, -1.50000000e-01], [ -1.11022302e-16, 5.00000000e-02, -1.00000000e-01], [ -1.11022302e-16, 5.00000000e-02, -5.00000000e-02], [ -1.11022302e-16, 5.00000000e-02, -5.55111512e-17], [ -1.11022302e-16, 5.00000000e-02, 5.00000000e-02], [ -1.11022302e-16, 5.00000000e-02, 1.00000000e-01], [ -1.11022302e-16, 1.00000000e-01, -2.00000000e-01], [ -1.11022302e-16, 1.00000000e-01, -1.50000000e-01], [ -1.11022302e-16, 1.00000000e-01, -1.00000000e-01], [ -1.11022302e-16, 1.00000000e-01, -5.00000000e-02], [ -1.11022302e-16, 1.00000000e-01, -5.55111512e-17], [ -1.11022302e-16, 1.00000000e-01, 5.00000000e-02], [ -1.11022302e-16, 1.00000000e-01, 1.00000000e-01], [ -1.11022302e-16, 1.50000000e-01, -2.00000000e-01], [ -1.11022302e-16, 1.50000000e-01, -1.50000000e-01], [ -1.11022302e-16, 1.50000000e-01, -1.00000000e-01], [ -1.11022302e-16, 1.50000000e-01, -5.00000000e-02], [ -1.11022302e-16, 1.50000000e-01, -5.55111512e-17], [ -1.11022302e-16, 1.50000000e-01, 5.00000000e-02], [ -1.11022302e-16, 1.50000000e-01, 1.00000000e-01], [ -1.11022302e-16, 2.00000000e-01, -2.00000000e-01], [ -1.11022302e-16, 2.00000000e-01, -1.50000000e-01], [ -1.11022302e-16, 2.00000000e-01, -1.00000000e-01], [ -1.11022302e-16, 2.00000000e-01, -5.00000000e-02], [ -1.11022302e-16, 2.00000000e-01, -5.55111512e-17], [ -1.11022302e-16, 2.00000000e-01, 5.00000000e-02], [ -1.11022302e-16, 2.00000000e-01, 1.00000000e-01], [ -1.11022302e-16, 2.50000000e-01, -1.50000000e-01], [ -1.11022302e-16, 2.50000000e-01, -1.00000000e-01], [ -1.11022302e-16, 2.50000000e-01, -5.00000000e-02], [ -1.11022302e-16, 2.50000000e-01, -5.55111512e-17], [ -1.11022302e-16, 2.50000000e-01, 5.00000000e-02], [ -1.11022302e-16, 2.50000000e-01, 1.00000000e-01], [ -1.11022302e-16, 3.00000000e-01, -5.00000000e-02], [ -1.11022302e-16, 3.00000000e-01, -5.55111512e-17], [ -1.11022302e-16, 3.00000000e-01, 5.00000000e-02], [ -1.11022302e-16, 3.00000000e-01, 1.00000000e-01], [ -1.11022302e-16, 3.50000000e-01, -5.55111512e-17], [ -1.11022302e-16, 3.50000000e-01, 5.00000000e-02], [ -1.11022302e-16, 3.50000000e-01, 1.00000000e-01], [ 5.00000000e-02, -5.00000000e-02, -1.00000000e-01], [ 5.00000000e-02, -5.00000000e-02, -5.00000000e-02], [ 5.00000000e-02, -5.00000000e-02, -5.55111512e-17], [ 5.00000000e-02, -5.00000000e-02, 5.00000000e-02], [ 5.00000000e-02, -5.00000000e-02, 1.00000000e-01], [ 5.00000000e-02, -1.11022302e-16, -1.00000000e-01], [ 5.00000000e-02, -1.11022302e-16, -5.00000000e-02], [ 5.00000000e-02, -1.11022302e-16, -5.55111512e-17], [ 5.00000000e-02, -1.11022302e-16, 5.00000000e-02], [ 5.00000000e-02, -1.11022302e-16, 1.00000000e-01], [ 5.00000000e-02, 5.00000000e-02, -1.00000000e-01], [ 5.00000000e-02, 5.00000000e-02, -5.00000000e-02], [ 5.00000000e-02, 5.00000000e-02, -5.55111512e-17], [ 5.00000000e-02, 5.00000000e-02, 5.00000000e-02], [ 5.00000000e-02, 5.00000000e-02, 1.00000000e-01], [ 5.00000000e-02, 1.00000000e-01, -2.00000000e-01], [ 5.00000000e-02, 1.00000000e-01, -1.50000000e-01], [ 5.00000000e-02, 1.00000000e-01, -1.00000000e-01], [ 5.00000000e-02, 1.00000000e-01, -5.00000000e-02], [ 5.00000000e-02, 1.00000000e-01, -5.55111512e-17], [ 5.00000000e-02, 1.00000000e-01, 5.00000000e-02], [ 5.00000000e-02, 1.00000000e-01, 1.00000000e-01], [ 5.00000000e-02, 1.50000000e-01, -2.00000000e-01], [ 5.00000000e-02, 1.50000000e-01, -1.50000000e-01], [ 5.00000000e-02, 1.50000000e-01, -1.00000000e-01], [ 5.00000000e-02, 1.50000000e-01, -5.00000000e-02], [ 5.00000000e-02, 1.50000000e-01, -5.55111512e-17], [ 5.00000000e-02, 1.50000000e-01, 5.00000000e-02], [ 5.00000000e-02, 1.50000000e-01, 1.00000000e-01], [ 5.00000000e-02, 2.00000000e-01, -2.00000000e-01], [ 5.00000000e-02, 2.00000000e-01, -1.50000000e-01], [ 5.00000000e-02, 2.00000000e-01, -1.00000000e-01], [ 5.00000000e-02, 2.00000000e-01, -5.00000000e-02], [ 5.00000000e-02, 2.00000000e-01, -5.55111512e-17], [ 5.00000000e-02, 2.00000000e-01, 5.00000000e-02], [ 5.00000000e-02, 2.00000000e-01, 1.00000000e-01], [ 5.00000000e-02, 2.50000000e-01, -1.50000000e-01], [ 5.00000000e-02, 2.50000000e-01, -1.00000000e-01], [ 5.00000000e-02, 2.50000000e-01, -5.00000000e-02], [ 5.00000000e-02, 2.50000000e-01, -5.55111512e-17], [ 5.00000000e-02, 2.50000000e-01, 5.00000000e-02], [ 5.00000000e-02, 2.50000000e-01, 1.00000000e-01], [ 5.00000000e-02, 3.00000000e-01, -5.00000000e-02], [ 5.00000000e-02, 3.00000000e-01, -5.55111512e-17], [ 5.00000000e-02, 3.00000000e-01, 5.00000000e-02], [ 5.00000000e-02, 3.00000000e-01, 1.00000000e-01], [ 5.00000000e-02, 3.50000000e-01, -5.55111512e-17], [ 5.00000000e-02, 3.50000000e-01, 5.00000000e-02], [ 5.00000000e-02, 3.50000000e-01, 1.00000000e-01], [ 1.00000000e-01, -5.00000000e-02, -5.55111512e-17], [ 1.00000000e-01, -5.00000000e-02, 5.00000000e-02], [ 1.00000000e-01, -5.00000000e-02, 1.00000000e-01], [ 1.00000000e-01, -1.11022302e-16, -5.55111512e-17], [ 1.00000000e-01, -1.11022302e-16, 5.00000000e-02], [ 1.00000000e-01, -1.11022302e-16, 1.00000000e-01], [ 1.00000000e-01, 5.00000000e-02, -5.55111512e-17], [ 1.00000000e-01, 5.00000000e-02, 5.00000000e-02], [ 1.00000000e-01, 5.00000000e-02, 1.00000000e-01], [ 1.00000000e-01, 1.00000000e-01, -1.00000000e-01], [ 1.00000000e-01, 1.00000000e-01, -5.00000000e-02], [ 1.00000000e-01, 1.00000000e-01, -5.55111512e-17], [ 1.00000000e-01, 1.00000000e-01, 5.00000000e-02], [ 1.00000000e-01, 1.00000000e-01, 1.00000000e-01], [ 1.00000000e-01, 1.50000000e-01, -2.00000000e-01], [ 1.00000000e-01, 1.50000000e-01, -1.50000000e-01], [ 1.00000000e-01, 1.50000000e-01, -1.00000000e-01], [ 1.00000000e-01, 1.50000000e-01, -5.00000000e-02], [ 1.00000000e-01, 1.50000000e-01, -5.55111512e-17], [ 1.00000000e-01, 1.50000000e-01, 5.00000000e-02], [ 1.00000000e-01, 1.50000000e-01, 1.00000000e-01], [ 1.00000000e-01, 2.00000000e-01, -2.00000000e-01], [ 1.00000000e-01, 2.00000000e-01, -1.50000000e-01], [ 1.00000000e-01, 2.00000000e-01, -1.00000000e-01], [ 1.00000000e-01, 2.00000000e-01, -5.00000000e-02], [ 1.00000000e-01, 2.00000000e-01, -5.55111512e-17], [ 1.00000000e-01, 2.00000000e-01, 5.00000000e-02], [ 1.00000000e-01, 2.00000000e-01, 1.00000000e-01], [ 1.00000000e-01, 2.50000000e-01, -1.00000000e-01], [ 1.00000000e-01, 2.50000000e-01, -5.00000000e-02], [ 1.00000000e-01, 2.50000000e-01, -5.55111512e-17], [ 1.00000000e-01, 2.50000000e-01, 5.00000000e-02], [ 1.00000000e-01, 2.50000000e-01, 1.00000000e-01], [ 1.00000000e-01, 3.00000000e-01, -5.55111512e-17], [ 1.00000000e-01, 3.00000000e-01, 5.00000000e-02], [ 1.00000000e-01, 3.00000000e-01, 1.00000000e-01], [ 1.00000000e-01, 3.50000000e-01, 5.00000000e-02], [ 1.00000000e-01, 3.50000000e-01, 1.00000000e-01], [ 1.50000000e-01, 1.50000000e-01, -2.00000000e-01], [ 1.50000000e-01, 1.50000000e-01, -5.55111512e-17], [ 1.50000000e-01, 1.50000000e-01, 5.00000000e-02], [ 1.50000000e-01, 2.00000000e-01, -2.00000000e-01], [ 1.50000000e-01, 2.00000000e-01, -1.50000000e-01], [ 1.50000000e-01, 2.00000000e-01, -1.00000000e-01], [ 1.50000000e-01, 2.00000000e-01, -5.00000000e-02], [ 1.50000000e-01, 2.00000000e-01, -5.55111512e-17], [ 1.50000000e-01, 2.00000000e-01, 5.00000000e-02], [ 1.50000000e-01, 2.50000000e-01, -1.00000000e-01], [ 1.50000000e-01, 2.50000000e-01, -5.00000000e-02], [ 1.50000000e-01, 2.50000000e-01, -5.55111512e-17], [ 1.50000000e-01, 2.50000000e-01, 5.00000000e-02], [ 1.50000000e-01, 3.00000000e-01, -5.55111512e-17], [ 1.50000000e-01, 3.00000000e-01, 5.00000000e-02], [ 1.50000000e-01, 3.50000000e-01, 5.00000000e-02], [ 2.00000000e-01, 1.50000000e-01, -2.00000000e-01], [ 2.00000000e-01, 2.00000000e-01, -1.50000000e-01], [ 2.00000000e-01, 2.50000000e-01, -5.00000000e-02], [ 2.00000000e-01, 2.50000000e-01, -5.55111512e-17], [ 2.00000000e-01, 3.00000000e-01, -5.55111512e-17]]); # print expected_feasible rows = np.size(expected_feasible, 0) cols = np.size(expected_feasible, 1) for i in range(0, rows): for j in range (0, cols): self.assertAlmostEquals(expected_feasible[i,j], feasible[i,j], self.assertPrecision)
else: swingIndex = iter ''' parameters to be tuned''' g = 9.81 mu = 0.8 axisZ = array([[0.0], [0.0], [1.0]]) n1 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n2 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n3 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n4 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # %% Cell 2 normals = np.vstack([n1, n2, n3, n4]) comp_dyn = ComputationalDynamics() comWF = np.array([0.0, 0.0, 0.0]) trunk_mass = 90 idx = 0 LF_tau_lim = [50.0, 100.0, 100.0] RF_tau_lim = [50.0, 100.0, 100.0] LH_tau_lim = [50.0, 100.0, 100.0] RH_tau_lim = [50.0, 100.0, 100.0] torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) params = IterativeProjectionParameters() params.setContactsPosWF(contacts) params.setCoMPosWF(comWF) params.setTorqueLims(torque_limits) params.setActiveContacts(stanceLegs)
def setup_iterative_projection(self, constraint_mode, contacts, comWF, trunk_mass, mu, normals): ''' parameters to be tuned''' g = 9.81 isOutOfWorkspace = False compDyn = ComputationalDynamics() grav = array([0., 0., -g]) contactsNumber = np.size(contacts, 0) # Unprojected state is: # # x = [f1_x, f1_y, f1_z, ... , f3_x, f3_y, f3_z] Ex = np.zeros((0)) Ey = np.zeros((0)) G = np.zeros((6, 0)) for j in range(0, contactsNumber): r = contacts[j, :] graspMatrix = compDyn.getGraspMatrix(r)[:, 0:3] Ex = hstack([Ex, -graspMatrix[4]]) Ey = hstack([Ey, graspMatrix[3]]) G = hstack([G, graspMatrix]) E = vstack((Ex, Ey)) / (g) f = zeros(2) proj = (E, f) # y = E * x + f # number of the equality constraints m_eq = 6 # see Equation (52) in "ZMP Support Areas for Multicontact..." A_f_and_tauz = array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 0, 0, 1]]) A = dot(A_f_and_tauz, G) t = hstack([0, 0, g, 0]) #print A,t eq = (A, t) # A * x == t act_LF = np.zeros((0, 1)) act_RF = np.zeros((0, 1)) act_LH = np.zeros((0, 1)) act_RH = np.zeros((0, 1)) actuation_polygons = np.zeros((0, 1)) # Inequality matrix for a contact force in local contact frame: constr = Constraints() #C_force = constr.linearized_cone_halfspaces(ng, mu) # Inequality matrix for stacked contact forces in world frame: if constraint_mode == 'ONLY_FRICTION': C, d = constr.linearized_cone_halfspaces_world( contactsNumber, ng, mu, normals) elif constraint_mode == 'ONLY_ACTUATION': #kin = Kinematics() kin = HyQKinematics() foot_vel = np.array([[0, 0, 0], [0, 0, 0], [0, 0, 0], [0, 0, 0]]) contactsFourLegs = np.vstack( [contacts, np.zeros((4 - contactsNumber, 3))]) q, q_dot, J_LF, J_RF, J_LH, J_RH, isOutOfWorkspace = kin.inverse_kin( np.transpose(contactsFourLegs[:, 0] - comWF[0]), np.transpose(foot_vel[:, 0]), np.transpose(contactsFourLegs[:, 1] - comWF[1]), np.transpose(foot_vel[:, 1]), np.transpose(contactsFourLegs[:, 2] - comWF[2]), np.transpose(foot_vel[:, 2])) J_LF, J_RF, J_LH, J_RH = kin.update_jacobians(q) if isOutOfWorkspace: C = np.zeros((0, 0)) d = np.zeros((1, 0)) else: act_LF = constr.computeLegActuationPolygon(J_LF) act_RF = constr.computeLegActuationPolygon(J_RF) act_LH = constr.computeLegActuationPolygon(J_LH) act_RH = constr.computeLegActuationPolygon(J_RH) ''' in the case of the IP alg. the contact force limits must be divided by the mass because the gravito inertial wrench is normalized''' C = np.zeros((0, 0)) d = np.zeros((1, 0)) actuation_polygons = np.array([act_LF, act_RF, act_LH, act_RH]) for j in range(0, contactsNumber): hexahedronHalfSpaceConstraints, knownTerm = constr.hexahedron( actuation_polygons[j] / trunk_mass) C = block_diag(C, hexahedronHalfSpaceConstraints) d = hstack([d, knownTerm.T]) d = d.reshape(6 * contactsNumber) #C = block_diag(c1, c2, c3, c4) #d = np.vstack([e1, e2, e3, e4]).reshape(6*4) #print C, d ineq = (C, d) # C * x <= d if isOutOfWorkspace: lp = 0 else: max_radius = 1e5 (E, f), (A, b), (C, d) = proj, ineq, eq assert E.shape[0] == f.shape[0] == 2 # Inequality constraints: A_ext * [ x u v ] <= b_ext iff # (1) A * x <= b and (2) |u|, |v| <= max_radius A_ext = zeros((A.shape[0] + 4, A.shape[1] + 2)) A_ext[:-4, :-2] = A A_ext[-4, -2] = 1 A_ext[-3, -2] = -1 A_ext[-2, -1] = 1 A_ext[-1, -1] = -1 A_ext = cvxopt.matrix(A_ext) b_ext = zeros(b.shape[0] + 4) b_ext[:-4] = b b_ext[-4:] = array([max_radius] * 4) b_ext = cvxopt.matrix(b_ext) # Equality constraints: C_ext * [ x u v ] == d_ext iff # (1) C * x == d and (2) [ u v ] == E * x + f C_ext = zeros((C.shape[0] + 2, C.shape[1] + 2)) C_ext[:-2, :-2] = C C_ext[-2:, :-2] = E[:2] C_ext[-2:, -2:] = array([[-1, 0], [0, -1]]) C_ext = cvxopt.matrix(C_ext) d_ext = zeros(d.shape[0] + 2) d_ext[:-2] = d d_ext[-2:] = -f[:2] d_ext = cvxopt.matrix(d_ext) lp_obj = cvxopt.matrix(zeros(A.shape[1] + 2)) lp = lp_obj, A_ext, b_ext, C_ext, d_ext return lp, actuation_polygons / trunk_mass, isOutOfWorkspace
def talker(): compDyn = ComputationalDynamics() math = Math() p=HyQSim() p.start() p.register_node() name = "Actuation_region" point = Point() actuationParams = ActuationParameters() i = 0 start_t_IP = time.time() for j in range (0,100): vertices = [point] # print("Time: " + str(i*0.004) + "s and Simulation time: " + str(p.get_sim_time()/60)) p.get_sim_wbs() actuationParams.getParams(p.hyq_rcf_debug) trunk_mass = 85. axisZ= np.array([[0.0], [0.0], [1.0]]) ''' normals ''' n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ)) normals = np.vstack([n1, n2, n3]) """ contact points """ nc = actuationParams.numberOfContacts contacts = actuationParams.contacts[0:nc+1, :] LF_tau_lim = [50.0, 100.0, 100.0] RF_tau_lim = [50.0, 100.0, 100.0] LH_tau_lim = [50.0, 100.0, 100.0] RH_tau_lim = [50.0, 100.0, 100.0] torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) comWF = np.array([0.0, 0.0, 0.0]) extForceW = np.array([0.0,0.0, 0.0]) constraint_mode_IP = ['FRICTION_AND_ACTUATION', 'FRICTION_AND_ACTUATION', 'FRICTION_AND_ACTUATION', 'FRICTION_AND_ACTUATION'] mu = 0.8 ng = 4 # print 'contacts: ',contacts # print contacts, actuationParams.stanceFeet params = IterativeProjectionParameters() stanceFeet = [1,1,1,1] params.setContactsPosWF(contacts) params.setCoMPosWF(comWF) params.setTorqueLims(torque_limits) params.setActiveContacts(stanceFeet) params.setConstraintModes(constraint_mode_IP) params.setContactNormals(normals) params.setFrictionCoefficient(mu) params.setNumberOfFrictionConesEdges(ng) params.setTotalMass(trunk_mass + extForceW[2]/9.81) params.externalForceWF = extForceW ''' compute iterative projection ''' IAR, actuation_polygons, computation_time = compDyn.iterative_projection_bretl(params) number_of_vertices = np.size(IAR, 0) # number_of_vertices = 10 # print IAR for i in range(0, number_of_vertices): point = Point() point.x = IAR[i][0] point.y = IAR[i][1] point.z = 0.0 vertices = np.hstack([vertices, point]) # print'vertices', vertices p.send_polygons(name, vertices) # time.sleep(1.0/5.0) i+=1 print 'de registering...' p.deregister_node() computation_time = (time.time() - start_t_IP) print("Total time: --- %s seconds ---" % computation_time) print 'number of published messages ', actuationParams.numberOfPublishedMessages avgTime = computation_time/actuationParams.numberOfPublishedMessages print 'average publishing time [ms]', avgTime print 'average publishing frequency [Hz]', 1.0/avgTime print 'number of received messages ', p.numberOfReceivedMessages avgTime = computation_time/p.numberOfReceivedMessages print 'average subscription time [ms]', avgTime print 'average subscription frequency [Hz]', 1.0/avgTime
def optimize_direction_variable_constraint(comWorld, vdir, solver=GLPK_IF_AVAILABLE): print 'I am hereeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeee' """ Optimize in one direction. Parameters ---------- vdir : (3,) array Direction in which the optimization is performed. lp : array tuple Tuple `(q, G, h, A, b)` defining the LP. See :func:`pypoman.lp..solve_lp` for details. solver : string, optional Backend LP solver to call. Returns ------- succ : bool Success boolean. z : (3,) array, or 0 Maximum vertex of the polygon in the direction `vdir`, or 0 in case of solver failure. """ """ contact points """ LF_foot = np.array([0.3, 0.3, -0.5]) RF_foot = np.array([0.3, -0.3, -0.5]) LH_foot = np.array([-0.3, 0.3, -0.5]) RH_foot = np.array([-0.3, -0.2, -0.5]) nc = 3 numberOfGenerators = 4 trunk_mass = 100 mu = 0.8 verbose = True contactsToStack = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot)) contactsWorldFrame = contactsToStack[0:nc, :] axisZ = array([[0.0], [0.0], [1.0]]) n1 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n2 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n3 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) n4 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) # %% Cell 2 normals = np.vstack([n1, n2, n3, n4]) iterProj = IterativeProjection() #comWorldFrame = np.array([0.0, 0.0, 0.0]) #comZero = np.array([0.0, 0.0, 0.0]) #comWorldFrame = np.array([0.28975694, 0.04463657, 0.0]) optimalSolutionFound = True comp_dyn = ComputationalDynamics() iter = 0 tol = 0.02 errorNorm = np.zeros((0, 1)) while (optimalSolutionFound) & (iter < 50): print "New tested CoM: ", comWorld iter += 1 p, G, h, A, b, isConstraintOk, LP_actuation_polygons = comp_dyn.setup_lp( trunk_mass, contactsWorldFrame, nc, numberOfGenerators, normals, comWorld, constraint_mode, mu) if not isConstraintOk: #unfeasible_points = np.vstack([unfeasible_points, com_WF]) if verbose: print 'something is wrong in the inequalities or the point is out of workspace' else: sol = solvers.lp(p, G, h, A, b) x = sol['x'] status = sol['status'] #print x if status == 'optimal': print "state is Feasible!" #feasible_points = np.vstack([feasible_points, com_WF]) #contact_forces = np.vstack([contact_forces, np.transpose(x)]) else: print "state is UNfeasible!" #unfeasible_points = np.vstack([unfeasible_points, com_WF]) new_lp, actuation_polygons, isOutOfWorkspace = iterProj.setup_iterative_projection( contactsWorldFrame, comWorld, trunk_mass, mu, normals) if isOutOfWorkspace: optimalSolutionFound = False print "Point is out of workspace!" else: lp_q, lp_Gextended, lp_hextended, lp_A, lp_b = new_lp lp_q[-2] = -vdir[0] lp_q[-1] = -vdir[1] x = solve_lp(lp_q, lp_Gextended, lp_hextended, lp_A, lp_b, solver=solver) tempSolution = x[-2:] forces = x[0:10] print "New solution point: ", tempSolution #print forces #print actuation_polygons[0:3] #print "LP actuation polygons: ", LP_actuation_polygons[0:3]/trunk_mass delta_x = tempSolution[0] - comWorld[0] delta_y = tempSolution[1] - comWorld[1] tempSolutionNorm = norm(tempSolution) comWorldFrameNorm = norm(comWorld[0:2]) errorNorm = np.vstack( [errorNorm, tempSolutionNorm - comWorldFrameNorm]) print 'eer', tempSolutionNorm - comWorldFrameNorm #if (np.abs(tempSolution[0])<np.abs(comWorldFrame[0]))|(np.abs(tempSolution[1])<np.abs(comWorldFrame[1])): if (np.abs(tempSolutionNorm - comWorldFrameNorm) < tol): optimalSolutionFound = False else: stepIncrement = 0.1 comWorld[0] += delta_x * stepIncrement comWorld[1] += delta_y * stepIncrement #return tempSolution, errorNorm return comWorld[0:2], errorNorm
max_iter -= 1 if len(init_vertices) < 3: raise Exception("problem is not linearly feasible") v0 = VertexVariableConstraints(init_vertices[0]) v1 = VertexVariableConstraints(init_vertices[1]) v2 = VertexVariableConstraints(init_vertices[2]) polygon = PolygonVariableConstraint() polygon.from_vertices(v0, v1, v2) polygon.iter_expand(max_iter) return polygon ''' MAIN ''' start_t_IPVC = time.time() math = Math() compDyn = ComputationalDynamics() # number of contacts nc = 3 # number of generators, i.e. rays used to linearize the friction cone ng = 4 # ONLY_ACTUATION or ONLY_FRICTION constraint_mode = 'ONLY_ACTUATION' useVariableJacobian = True trunk_mass = 100 mu = 0.8 # number of decision variables of the problem n = nc * 6 """ contact points """ LF_foot = np.array([0.3, 0.3, -0.5]) RF_foot = np.array([0.3, -0.3, -0.5])
n4 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ)) normals = np.vstack([n1, n2, n3, n4]) LF_tau_lim = [50.0, 100.0, 100.0] RF_tau_lim = [50.0, 100.0, 100.0] LH_tau_lim = [50.0, 100.0, 100.0] RH_tau_lim = [50.0, 100.0, 100.0] torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) comWF = np.array([0.0, -0.3, 0.0]) # fig = plt.figure() nc = np.sum(stanceFeet) plt.plot(contacts[0:nc, 0], contacts[0:nc, 1], 'ko', markersize=15) comp_dyn = ComputationalDynamics() params = IterativeProjectionParameters() params.setContactsPosBF(contacts) params.setCoMPosWF(comWF) params.setTorqueLims(torque_limits) params.setActiveContacts(stanceFeet) params.setConstraintModes(constraint_mode_IP) params.setContactNormals(normals) params.setFrictionCoefficient(mu) params.setNumberOfFrictionConesEdges(ng) params.setTotalMass(trunk_mass) ''' compute iterative projection ''' feasible_points = np.zeros((0, 3)) unfeasible_points = np.zeros((0, 3)) contact_forces = np.zeros((0, nc * 3))
''' torque limits for each leg (this code assumes a hyq-like design, i.e. three joints per leg) HAA = Hip Abduction Adduction HFE = Hip Flextion Extension KFE = Knee Flextion Extension ''' LF_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE RF_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE LH_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE RH_tau_lim = [50.0, 100.0, 100.0] # HAA, HFE, KFE torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) ''' extForceW is an optional external pure force (no external torque for now) applied on the CoM of the robot.''' extForceW = np.array([0.0, 0.0, 0.0]) # units are Nm comp_dyn = ComputationalDynamics() '''You now need to fill the 'params' object with all the relevant informations needed for the computation of the IP''' params = IterativeProjectionParameters() params.setContactsPosWF(contacts) params.setCoMPosWF(comWF) params.setTorqueLims(torque_limits) params.setActiveContacts(stanceFeet) params.setConstraintModes(constraint_mode_IP) params.setContactNormals(normals) params.setFrictionCoefficient(mu) params.setNumberOfFrictionConesEdges(ng) params.setTotalMass(trunk_mass) params.externalForceWF = extForceW # params.externalForceWF is actually used anywhere at the moment