As shown in the figure, sub-figures 1~4, 6 ,7 match joints 1 ~ 4, 6 and 7, they are the joints I want to control, and joint 5 is in free mode. The blue line is the target velocity, and the red line is the measured velocity (the blue line in sub-figure 5 is the expected response, and the red line is the actual response). Obviously, joint 2, 4 does not accurately reach the target velocity. At first I suspected that the maximum torque of the joint was too small, so I set the maximum torque to 1e+20, but it was still the result in the picture. Here is my code.
Code: Select all
#python
import math
import numpy as np
import matplotlib.pyplot as plt
Pi = math.pi
def sysCall_init():
sim = require("sim")
self.base=sim.getObject(".")
self.j1 = sim.getObject("/j_1")
self.j2 = sim.getObject("/j_2")
self.j3 = sim.getObject("/j_3")
self.j4 = sim.getObject("/j_4")
self.j5 = sim.getObject("/j_5")
self.j6 = sim.getObject("/j_6")
self.j7 = sim.getObject("/j_7")
self.graph=sim.getObject("/Graph")
self.v1 = sim.addGraphStream(self.graph, 'v1', 'rad/s', 0, [1, 0, 0])
sim.setStepping(True)
def sysCall_sensing():
#print('setGraphStreamValue')
pass
def Read_txt(str):
with open(str, 'r') as f:
lines = f.readlines()
float_data = [[float(num) for num in line.split()] for line in lines]
return float_data
def sysCall_thread():
thtpath='G:/Reliable_fault-tolerant_planning_domain/Sudden_failure/tht_cal.txt'
ins_v=Read_txt(thtpath)
step=len(ins_v)
j_num=7
t0=0.05
i=1
eulerAngles=[0,-Pi/2,0]
tht_act=np.zeros((step,7))
print(sim.getSimulationTimeStep())
while not sim.getSimulationStopping():
#print(i)
if i==step:
break
sim.setJointTargetVelocity(self.j1, ins_v[i][0],[])
sim.setJointTargetVelocity(self.j2, ins_v[i][1],[])
sim.setJointTargetVelocity(self.j3, ins_v[i][2],[])
sim.setJointTargetVelocity(self.j4, ins_v[i][3],[])
sim.setJointTargetVelocity(self.j6, ins_v[i][5],[])
sim.setJointTargetVelocity(self.j7, ins_v[i][6],[])
sim.setObjectOrientation(self.base, eulerAngles, sim.handle_world)
tht_act[i][0]=sim.getJointVelocity(self.j1)
tht_act[i][1]=sim.getJointVelocity(self.j2)
tht_act[i][2]=sim.getJointVelocity(self.j3)
tht_act[i][3]=sim.getJointVelocity(self.j4)
tht_act[i][4]=sim.getJointVelocity(self.j5)
tht_act[i][5]=sim.getJointVelocity(self.j6)
tht_act[i][6]=sim.getJointVelocity(self.j7)
sim.setGraphStreamValue(self.graph, self.v1, sim.getJointVelocity(self.j1))
i=i+1
print(i)
sim.step()
plt.figure
plt.subplot(2,4,1)
plt.plot(np.arange(0,step,1),[row[0] for row in ins_v])
plt.plot(np.arange(0,step,1),[row[0] for row in tht_act])
plt.subplot(2,4,2)
plt.plot(np.arange(0,step,1),[row[1] for row in ins_v])
plt.plot(np.arange(0,step,1),[row[1] for row in tht_act])
plt.subplot(2,4,3)
plt.plot(np.arange(0,step,1),[row[2] for row in ins_v])
plt.plot(np.arange(0,step,1),[row[2] for row in tht_act])
plt.subplot(2,4,4)
plt.plot(np.arange(0,step,1),[row[3] for row in ins_v])
plt.plot(np.arange(0,step,1),[row[3] for row in tht_act])
plt.subplot(2,3,4)
plt.plot(np.arange(0,step,1),[row[4] for row in ins_v])
plt.plot(np.arange(0,step,1),[row[4] for row in tht_act])
plt.subplot(2,3,5)
plt.plot(np.arange(0,step,1),[row[5] for row in ins_v])
plt.plot(np.arange(0,step,1),[row[5] for row in tht_act])
plt.subplot(2,3,6)
plt.plot(np.arange(0,step,1),[row[6] for row in ins_v])
plt.plot(np.arange(0,step,1),[row[6] for row in tht_act])
plt.show()