def execute(self, userdata):
start = []
for q in userdata.q[0:6]:
start.append(np.array([q]))
self.runner.update_start(start)
while self.count < self.runner.get_length():
rospy.loginfo(self.model_name + " is at " + str(self.count) )
self.runner.step()
msg = DesiredJoints()
q = self.runner.x
qd = self.runner.dx
qdd = self.runner.ddx
size_diff = abs(len(q) - len(self.joint_state.position))
q = np.append(q, size_diff*[0.0])
qd = np.append(qd, size_diff*[0.0])
qdd = np.append(qdd, size_diff*[0.0])
msg.q = q
msg.qd = qd
msg.qdd = qdd
msg.controller = self._controller_name
self.pub.publish(msg)
self.count += 1
# print(count)
userdata.human = False
self.rate.sleep()
return "walked"
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):
msg_pos = DesiredPosCmd()
msg_pos.pos.x = 0.0
msg_pos.pos.y = 0.0
msg_pos.pos.z = 3.0
msg_pos.rpy.x = 0.25
msg_pos.rpy.y = 0.0
msg_pos.rpy.z = 0.0
self.pub_pos.publish(msg_pos)
rospy.wait_for_service('exo_onoff')
try:
exo = rospy.ServiceProxy('exo_onoff', SetBool)
resp1 = exo(True)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
self.count = 0
while self.count <= self.total - 1:
msg = DesiredJoints()
q = np.array([
self.hip["q"][self.count].item(),
self.knee["q"][self.count].item(),
self.ankle["q"][self.count].item(),
self.hip["q"][self.count].item(),
self.knee["q"][self.count].item(),
self.ankle["q"][self.count].item(), 0.0
])
qd = np.array([
self.hip["qd"][self.count].item(),
self.knee["qd"][self.count].item(),
self.ankle["qd"][self.count].item(),
self.hip["qd"][self.count].item(),
self.knee["qd"][self.count].item(),
self.ankle["qd"][self.count].item(), 0.0
])
qdd = np.array([
self.hip["qdd"][self.count].item(),
self.knee["qdd"][self.count].item(),
self.ankle["qdd"][self.count].item(),
self.hip["qdd"][self.count].item(),
self.knee["qdd"][self.count].item(),
self.ankle["qdd"][self.count].item(), 0.0
])
self.count += 1
msg.q = q
msg.qd = qd
msg.qdd = qdd
msg.controller = "Dyn"
self.pub_joints.publish(msg)
self.rate.sleep()
return "Initialized"
def execute(self, userdata):
q = userdata.q
msg = DesiredJoints()
count = self.count
if count <= len(q[0]["q"]) - 1:
q_d = np.array([
q[0]["q"][count].item(), q[1]["q"][count].item(),
q[2]["q"][count].item(), q[3]["q"][count].item(),
q[4]["q"][count].item(), q[5]["q"][count].item(), 0.0
])
qd_d = np.array([
q[0]["qd"][count].item(), q[1]["qd"][count].item(),
q[2]["qd"][count].item(), q[3]["qd"][count].item(),
q[4]["qd"][count].item(), q[5]["qd"][count].item(), 0.0
])
qdd_d = np.array([
q[0]["qdd"][count].item(), q[1]["qdd"][count].item(),
q[2]["qdd"][count].item(), q[3]["qdd"][count].item(),
q[4]["qdd"][count].item(), q[5]["qdd"][count].item(), 0.0
])
msg.q = q_d
msg.qd = qd_d
msg.qdd = qdd_d
msg.controller = "Dyn"
#self.send(q_d, qd_d, qdd_d,"Dyn", [])
self.pub.publish(msg)
self.count += 1
self.rate.sleep()
return "Following"
else:
self.count = 0
return "Followed"
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"