def __init__(self, model, outcomes=["standing", "stood"]):
smach.State.__init__(self, outcomes=outcomes)
rospy.wait_for_service('joint_cmd')
self.send = rospy.ServiceProxy('joint_cmd', DesiredJointsCmd)
self._model = model
self.rate = rospy.Rate(10)
self.tf = 2.0
self.dt = 0.01
# ip = self._model.q[3:6]
# self.hip, self.knee, self.ankle = self._model.standing_to_sitting_trajectory(ip)
# print(self.hip)
# print(self.knee)
# print(self.ankle)
self.total = self.tf / self.dt
self.set_msg = DesiredJoints()
self.error_msg = DesiredJoints()
self.set_pub = rospy.Publisher(self._model.model_name + "_set_points",
DesiredJoints,
queue_size=1)
self.error_pub = rospy.Publisher(self._model.model_name + "_error",
DesiredJoints,
queue_size=1)
self.state_pub = rospy.Publisher("state", String, queue_size=1)
# self.restart_sub = rospy.Subscriber("restart", String, self.restart_cb)
self.start = ""
self.count = 0
self.num_cycles = 0
self.execute_start = True
self.last_x = []
self.ip = []
def __init__(self, model, outcomes=["stepping", "stepped"]):
smach.State.__init__(self, outcomes=outcomes)
self._model = model
self.runner = self._model.get_runner()
self.rate = rospy.Rate(100)
self.msg = DesiredJoints()
self.pub = rospy.Publisher("set_points", DesiredJoints, queue_size=1)
self.count = 0
def __init__(self, model, outcomes=["Sending", "Waiting"]):
smach.State.__init__(self, outcomes=outcomes)
rospy.Subscriber("Traj", DesiredJoints, callback=self.traj_cb)
self._model = model
self.have_msg = False
self.Rate = rospy.Rate(100)
self.q = DesiredJoints()
self.pub = rospy.Publisher("set_points", DesiredJoints, queue_size=1)
def __init__(self, model, outcomes=["stepping", "stepped"]):
smach.State.__init__(self, outcomes=outcomes)
rospy.wait_for_service(model.model_name + '_joint_cmd')
self.send = rospy.wait_for_service(model.model_name + '_joint_cmd')
self._model = model
self.runner = self._model.get_runner()
self.rate = rospy.Rate(100)
self.msg = DesiredJoints()
self.pub = rospy.Publisher("set_points", DesiredJoints, queue_size=1)
self.count = 0
def __init__(self, model, outcomes=['Following', 'Followed']):
smach.State.__init__(self,
outcomes=outcomes,
input_keys=['q'],
output_keys=['q'])
self._model = model
self.rate = rospy.Rate(100)
self.msg = DesiredJoints()
self.pub = rospy.Publisher("set_points", DesiredJoints, queue_size=1)
self.count = 0
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):
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 __init__(self, model, outcomes=["walking", "walked"]):
smach.State.__init__(self, outcomes=outcomes)
rospy.wait_for_service('joint_cmd')
self.send = rospy.ServiceProxy('joint_cmd', DesiredJointsCmd)
self._model = model
self.runner = self._model.get_walker()
self.rate = rospy.Rate(1000)
self.set_msg = DesiredJoints()
self.error_msg = DesiredJoints()
self.set_pub = rospy.Publisher(self._model.model_name + "_set_points",
DesiredJoints,
queue_size=1)
self.error_pub = rospy.Publisher(self._model.model_name + "_error",
DesiredJoints,
queue_size=1)
self.state_pub = rospy.Publisher("state", String, queue_size=1)
self.restart_sub = rospy.Subscriber("restart", String, self.restart_cb)
self.start = ""
self.count = 0
self.num_cycles = 0
self.execute_start = True
self.last_x = []
def __init__(self, model, outcomes=['Initializing', 'Initialized']):
smach.State.__init__(self, outcomes=outcomes)
rospy.wait_for_service('joint_cmd')
self._model = model
self.rate = rospy.Rate(100)
tf = 2.0
dt = 0.01
self.hip, self.knee, self.ankle = self._model.stance_trajectory(tf=tf, dt=dt)
self.msg = DesiredJoints()
self.pub = rospy.Publisher(self._model.model_name + "_set_points", DesiredJoints, queue_size=1)
self.total = tf / dt
self.count = 0
def execute(self, userdata):
# Your state execution goes here
self.Rate.sleep()
if self.have_msg:
q_d = np.array(list(self.q.q) + [0.0])
qd_d = np.array(list(self.q.qd) + [0.0])
qdd_d = np.array(list(self.q.qdd) + [0.0])
self.msg = DesiredJoints()
self.msg.q = q_d
self.msg.qd = qd_d
self.msg.qdd = qdd_d
self.pub.publish(self.msg)
self.have_msg = False
return "Sending"
else:
return "Waiting"
def __init__(self, model, outcomes=["stairing", "staired"]):
smach.State.__init__(self, outcomes=outcomes)
rospy.wait_for_service(model.model_name + '_joint_cmd')
self.send = rospy.wait_for_service(model.model_name + '_joint_cmd')
self._model = model
project_root = dirname(dirname(__file__))
fileZ = join(project_root, 'config/toeZ_all.pickle')
fileY = join(project_root, 'config/toeY_all.pickle')
self.runnerZ = GMMRunner.GMMRunner(
fileZ) # make_toeZ([file1, file2], hills3, nb_states, "toe_IK")
self.runnerY = GMMRunner.GMMRunner(
fileY) # make_toeY([file1, file2], hills3, nb_states, "toe_IK")
self.rate = rospy.Rate(10)
self.msg = DesiredJoints()
self.pub = rospy.Publisher("set_points", DesiredJoints, queue_size=1)
self.count = 0
self.init_joint_angles = []
def __init__(self,
model_name,
outcomes=['WalkInitialized'],
output_keys=['q', 'qd', 'human']):
smach.State.__init__(self, outcomes=outcomes, output_keys=output_keys)
self.rate = rospy.Rate(100)
tf = 2.0
dt = 0.01
self.joint_state = JointState()
self.joint_cb = rospy.Subscriber(model_name + "_jointstate",
JointState, self.joint_callback)
self.msg = DesiredJoints()
self.pub = rospy.Publisher(model_name + "_set_points",
DesiredJoints,
queue_size=1)
self.total = tf / dt
self.count = 0
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"
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 traj_cb(self, msg):
self.q = DesiredJoints()
if not self.have_msg:
self.q = msg
self.have_msg = True
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"