np.argsort(comTrajectoriesToStack[:, 0], axis=None),
comTrajectoriesToStack[:, 0].shape)
max_motin_index = max_motion_indices[0][0]
print("Directed Iterative Projection: --- %s seconds ---" %
(time.time() - start_t_IPVC))
''' plotting Iterative Projection points '''
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])
IP_points, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(
constraint_mode, contacts, normals, trunk_mass, ng, mu)
#IP_points_saturated_friction, actuation_polygons = compDyn.iterative_projection_bretl('ONLY_FRICTION', contacts, normals, trunk_mass, ng, mu, saturate_normal_force = True)
'''Plotting'''
plt.close('all')
plotter = Plotter()
plt.figure()
plt.grid()
plt.xlabel("X [m]")
plt.ylabel("Y [m]")
plt.plot(comTrajectoriesToStack[:, 0],
comTrajectoriesToStack[:, 1],
'g^',
markersize=20)
params.setCoMPos(comWF)
params.setTorqueLims(torque_limits)
params.setActiveContacts(stanceFeet)
params.setConstraintModes(constraint_mode_IP)
params.setContactNormals(normals)
params.setFrictionCoefficient(mu)
params.setNumberOfFrictionConesEdges(ng)
params.setTrunkMass(trunk_mass)
feasible, unfeasible, contact_forces = compDyn.LP_projection(
params, useVariableJacobian, 0.2, 0.2)
print feasible
print unfeasible
#desired_direction = [-1.0, -1.0]
IP_points1, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(
params)
comWF = np.array([-0.205, 0.1, 0.0])
params.setCoMPos(comWF)
IP_points2, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(
params)
comWF = np.array([-0.131, 0.1, 0.0])
params.setCoMPos(comWF)
IP_points3, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(
params)
comWF = np.array([-0.17, 0.1, 0.0])
params.setCoMPos(comWF)
IP_points4, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(
params)
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
for com_x in range(lowel_lim, upper_lim, 10):
# print com_x/100.0
comWF = np.array([com_x / 100.0, 0.0, 0.0])
# print comWF, comToStack
comToStackX = np.hstack([comToStackX, comWF[0]])
comToStackY = np.hstack([comToStackY, comWF[1]])
# params.setCoMPos(comWF)
LF_foot_tmp = LF_foot - comWF
RF_foot_tmp = RF_foot - comWF
LH_foot_tmp = LH_foot - comWF
RH_foot_tmp = RH_foot - comWF
contactsBF = np.vstack(
(LF_foot_tmp, RF_foot_tmp, LH_foot_tmp, RH_foot_tmp))
params.setContactsPosWF(contactsBF)
IP_points, actuation_polygons, comp_time = comp_dyn.iterative_projection_bretl(
params)
point = np.vstack([IP_points])
# print idx, scale
# print scale[idx]
colorVal = scalarMap.to_rgba(scale[idx])
colorText = ('color: (%4.2f,%4.2f,%4.2f)' %
(colorVal[0], colorVal[1], colorVal[2]))
idx += 1
#plotter.plot_polygon(np.transpose(IP_points), x[0],'trunk mass ' + str(trunk_mass*10) + ' N')
x = np.hstack([point[:, 0], point[0, 0]])
y = np.hstack([point[:, 1], point[0, 1]])
h = plt.plot(x,
y,
color=colorVal,
linewidth=5.,
label=str(com_x / 100.0) + ' cm')
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 ''' compute iterative projection Outputs of "iterative_projection_bretl" are: IP_points = resulting 2D vertices actuation_polygons = these are the vertices of the 3D force polytopes (one per leg) computation_time = how long it took to compute the iterative projection ''' IP_points, force_polytopes, IP_computation_time = comp_dyn.iterative_projection_bretl(params) ''' plotting Iterative Projection points ''' feasible, unfeasible, contact_forces = comp_dyn.LP_projection(params) #print contact_forces #for i in range(0, np.size(contact_forces,0)): # for j in range(0,nc): # a = Arrow3D([contacts[j,0], contacts[j,0]+contact_forces[i,j*3]/200], [contacts[j,1], contacts[j,1]+contact_forces[i,j*3+1]/200],[contacts[j,2], contacts[j,2]+contact_forces[i,j*3+2]/200], mutation_scale=20, lw=3, arrowstyle="-|>", color="g") # ax.add_artist(a) #a1 = Arrow3D([0.0, 0.0],[ 0.0,0.0],[ 0.0, -trunk_mass*g/scaling_factor], mutation_scale=20, lw=3, arrowstyle="-|>", color="b") #ax.add_artist(a1) ''' plotting LP test points ''' #if np.size(feasible,0) != 0:
