def execute(self, userdata):
if self.count < self.runner.get_length():
self.runner.step()
x = self.runner.x
dx = self.runner.dx
ddx = self.us2[self.count]
q = np.append(x, [0.0])
qd = np.append(dx, [0.0])
qdd = np.append(ddx, [0.0])
msg = DesiredJoints()
msg.q = q.tolist()
msg.qd = qd.tolist()
msg.qdd = qdd.tolist()
msg.other = q.tolist()
msg.controller = "FF"
self.send(q, qd, qdd, "FF", [self.count])
msg = DesiredJoints()
joints = DesiredJoints()
msg.q = q.tolist()
joints.q = self._model.q.tolist()
self.pub.publish(msg)
self.my_joints.publish(joints)
self.rate.sleep()
self.count += 1
return "LQRing"
else:
return "LQRed"
def execute(self, userdata):
ilqr_tau_msg = Float32MultiArray()
start = []
for q in userdata.q[0:6]:
start.append(np.array([q]))
self.count = 0
self.runner = self._get_walker()
self.runner.update_start(start)
x = np.array(self.runner.x)
dx = np.array(self.runner.dx)
rospy.sleep(1.0)
while self.count < self.runner.get_length():
self.runner.step()
x = np.array(self.runner.x) #+ 0.2*x
dx = np.array(self.runner.dx) #+ 0.2*dx
ddx = -self.ilqr_tau[self.count]
ilqr_tau_msg.data = ddx
size_diff = abs(len(x) - len(self.joint_state.position))
q = np.append(x, size_diff * [0.0])
qd = np.append(dx, size_diff * [0.0])
qdd = np.append(ddx, size_diff * [0.0])
msg = DesiredJoints()
msg.q = q.tolist()
msg.qd = qd.tolist()
msg.qdd = qdd.tolist()
msg.other = q.tolist()
msg.controller = self._controller_name
msg.q = q.tolist()
self.pub.publish(msg)
self.pub_tau_ilqr.publish(ilqr_tau_msg)
self.count += 1
self.rate.sleep()
rospy.sleep(2.0)
return "ilqred"