def pubish_signal(self, motor_id, output, speed=300):
# motor_id: 1-4 corresponds to chassis 1-4, 5-8 corresponds to tibia 1-4
# speed range: 0-1023
# output: a tuple of 8 values for each motor. range: 0-1023
pub = rospy.Publisher("motor_signal", MotorSignal, queue_size=10)
motor_signal = MotorSignal()
motor_signal.speed = speed
motor_signal.motor_id = motor_id
motor_signal.signal = output
pub.publish(motor_signal)
def main():
rospy.init_node('spyndra', anonymous=True)
rospy.Subscriber('/motor_signal', MotorSignal, callback)
pub = rospy.Publisher('/actual_signal', MotorSignal, queue_size=10)
motors = ax12.Ax12(port=PORT)
while not rospy.is_shutdown():
#keep publish current position
for i in range(8):
'''get actual position of the motor, and then publish it back to control_node'''
'''right now i simply set them as dummy'''
motor_id = i + 1
motor_state = MotorSignal()
motor_state.motor_id = i + 1
motor_state.speed = ax12.readPresentSpeed(motor_id)
motor_state.signal = ax12.readPosition(motor_id)
motor_state.load = ax12.readLoad(motor_id)
pub.publish(motor_state)
rospy.loginfo(motor_state)
def _reset(self):
# Unpause simulation to make observation
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except (rospy.ServiceException) as e:
print("/gazebo/unpause_physics service call failed")
try:
# Wait for robot to be ready to accept signal
rospy.wait_for_message('motor_state', MotorSignal, timeout=5)
motor_signal = MotorSignal()
motor_signal.motor_type = 1
motor_signal.signal = [512, 512, 512, 512, 512, 512, 512, 512]
self.action_publisher.publish(motor_signal)
except:
print("cannot publish action")
time.sleep(.5)
# return to initial joint positions
rospy.wait_for_service('/gazebo/set_model_configuration')
try:
joint_names = [
'base_to_femur_1', 'base_to_femur_2', 'base_to_femur_3',
'base_to_femur_4', 'femur_to_tibia_1', 'femur_to_tibia_2',
'femur_to_tibia_3', 'femur_to_tibia_4'
]
joint_positions = [0, 0, 0, 0, 0, 0, 0, 0]
self.set_model_config_proxy('spyndra', 'spyndra_description',
joint_names, joint_positions)
except (rospy.ServiceException) as e:
print("/gazebo/set_model_configuration service call failed")
time.sleep(.5)
# return to initial model position
rospy.wait_for_service('/gazebo/set_model_state')
try:
model_state = ModelState()
model_state.model_name = 'spyndra'
model_state.pose.position.x, model_state.pose.position.y, model_state.pose.position.z = 0, 0, 0.5
model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, model_state.pose.orientation.w = 0, 0, 0, 0
model_state.twist.linear.x, model_state.twist.linear.y, model_state.twist.linear.z = 0, 0, 0
model_state.twist.angular.x, model_state.twist.angular.y, model_state.twist.angular.z = 0, 0, 0
self.set_model_state_proxy(model_state)
except (rospy.ServiceException) as e:
print("/gazebo/set_model_state service call failed")
time.sleep(1)
# stand up
try:
# Wait for robot to be ready to accept signal
rospy.wait_for_message('motor_state', MotorSignal, timeout=5)
motor_signal = MotorSignal()
motor_signal.motor_type = 1
motor_signal.signal = [512, 512, 512, 512, 820, 820, 820, 820]
self.action_publisher.publish(motor_signal)
except:
print("cannot publish action")
time.sleep(7)
# initiate observation
s_ = np.zeros(self.nS)
# older gazebo version always have different initial IMU info after reset, keep them zero
# # imu data update
# imu_data = None
# while imu_data is None:
# try:
# imu_data = rospy.wait_for_message('imu', Imu)
# s_[8: 14] = [imu_data.linear_acceleration.x, imu_data.linear_acceleration.y, imu_data.linear_acceleration.z, \
# imu_data.angular_velocity.x, imu_data.angular_velocity.y, imu_data.angular_velocity.z]
# except:
# pass
# motor data update
motor_data = None
while motor_data is None:
try:
motor_data = rospy.wait_for_message('motor_state', MotorSignal)
s_[:8] = motor_data.signal
except:
pass
# position data update
position_data = None
while position_data is None:
try:
position_data = rospy.wait_for_message('/gazebo/model_states',
ModelStates,
timeout=5)
index = position_data.name.index("spyndra")
#s_[8: 11] =
x, y, z = [
position_data.pose[index].position.x,
position_data.pose[index].position.y,
position_data.pose[index].position.y
]
self.INIT_POS = np.array([x, y, z])
self.GOAL = np.array(self.INIT_POS, copy=True)
self.GOAL[0] += 10
self.INIT_DIST = np.linalg.norm(self.INIT_POS - self.GOAL)
except:
pass
rospy.wait_for_service('/gazebo/pause_physics')
try:
#resp_pause = pause.call()
self.pause()
except (rospy.ServiceException) as e:
print("/gazebo/pause_physics service call failed")
self.START_TIME = time.time()
return s_
def _step(self, action, s):
'''
take action and update the observation(state)
'''
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except (rospy.ServiceException) as e:
print("/gazebo/unpause_physics service call failed")
s_ = s.copy()
# if the observation contains previous state
if len(s) > 14:
s_ = np.hstack((np.zeros(14), s_[:-14]))
# update motor position
self.torque = 0.
try:
motor_signal = MotorSignal()
motor_signal.motor_type = 1
s_[:8] = action
for i in range(4):
s_[i] = self.clip_signal(s_[i], 350, 630)
for i in range(4, 8, 1):
s_[i] = self.clip_signal(s_[i], 650, 890)
motor_signal.signal = action
self.action_publisher.publish(motor_signal)
except:
print("cannot publish action")
s_time = time.time()
# imu data update
imu_data = None
while imu_data is None:
try:
imu_data = rospy.wait_for_message('imu', Imu, timeout=5)
s_[8: 14] = [imu_data.linear_acceleration.x, imu_data.linear_acceleration.y, imu_data.linear_acceleration.z, \
imu_data.angular_velocity.x, imu_data.angular_velocity.y, imu_data.angular_velocity.z]
except:
pass
# joint position update
motor_data = None
while (time.time() - s_time) < 1.8: # large gates take longer time
#motor_data = None
#while motor_data is None:
try:
motor_data = rospy.wait_for_message('motor_state',
MotorSignal,
timeout=5)
s_[:8] = np.array(motor_data.signal)
except:
pass
# position data update
position_data = None
while position_data is None:
try:
position_data = rospy.wait_for_message('/gazebo/model_states',
ModelStates,
timeout=5)
index = position_data.name.index("spyndra")
position_data = [
position_data.pose[index].position.x,
position_data.pose[index].position.y,
position_data.pose[index].position.y
]
except:
pass
rospy.wait_for_service('/gazebo/pause_physics')
try:
self.pause()
except (rospy.ServiceException) as e:
print("/gazebo/pause_physics service call failed")
# Reward Function
## Time reward
time_reward = 10 - .2 * (time.time() - self.START_TIME)
#time_reward = 0.
## Distance reward
curr_dist2goal = np.linalg.norm(np.array(position_data) - self.GOAL)
dist_reward = (self.INIT_DIST - curr_dist2goal) * 15. / self.INIT_DIST
#print "dist to goal:", curr_dist2goal, "dist reward:", dist_reward, "motor signal:", s_[:8]
## Angel reward
angl_reward = 0
#angl_reward = (2 * np.pi - angl2goal) * 15. / (2 * np.pi)
## Torque reward
torq_reward = -self.torque * 5
reward = time_reward + dist_reward * 5 + torq_reward + angl_reward
# threshold TBD
if curr_dist2goal < 1:
done = True
else:
done = False
return s_, reward, done, curr_dist2goal
def spyndra_joint_positions_publisher():
#Initiate node for controlling joint positions.
rospy.init_node('joint_positions_node', anonymous=True)
#Define publishers for each joint position controller commands.
b2f_1 = rospy.Publisher(
'/spyndra/base_to_femur_1_position_controller/command',
Float64,
queue_size=10)
b2f_2 = rospy.Publisher(
'/spyndra/base_to_femur_2_position_controller/command',
Float64,
queue_size=10)
b2f_3 = rospy.Publisher(
'/spyndra/base_to_femur_3_position_controller/command',
Float64,
queue_size=10)
b2f_4 = rospy.Publisher(
'/spyndra/base_to_femur_4_position_controller/command',
Float64,
queue_size=10)
f2t_1 = rospy.Publisher(
'/spyndra/femur_to_tibia_1_position_controller/command',
Float64,
queue_size=10)
f2t_2 = rospy.Publisher(
'/spyndra/femur_to_tibia_2_position_controller/command',
Float64,
queue_size=10)
f2t_3 = rospy.Publisher(
'/spyndra/femur_to_tibia_3_position_controller/command',
Float64,
queue_size=10)
f2t_4 = rospy.Publisher(
'/spyndra/femur_to_tibia_4_position_controller/command',
Float64,
queue_size=10)
action_publisher = rospy.Publisher('motor_signal',
MotorSignal,
queue_size=5)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
line = sys.stdin.readline()
pos = [float(p) for p in line.split()]
motor_signal = MotorSignal()
motor_signal.motor_type = 1
motor_signal.signal = pos[:8]
action_publisher.publish(motor_signal)
# b2f_1.publish(pos[0])
# b2f_2.publish(pos[1])
# b2f_3.publish(pos[2])
# b2f_4.publish(pos[3])
# f2t_1.publish(pos[4])
# f2t_2.publish(pos[5])
# f2t_3.publish(pos[6])
# f2t_4.publish(pos[7])
rate.sleep()
def _reset(self):
# Unpause simulation to make observation
# rospy.wait_for_service('/gazebo/unpause_physics')
# try:
# #resp_pause = pause.call()
# self.unpause()
# except (rospy.ServiceException) as e:
# print ("/gazebo/unpause_physics service call failed")
# try:
# Wait for robot to be ready to accept signal
# rospy.wait_for_message('/motor_state', MotorSignal, timeout=5)
# motor_signal = MotorSignal()
# motor_signal.motor_type = 1
# motor_signal.signal = [512, 512, 512, 512, 512, 512, 512, 512]
# self.action_publisher.publish(motor_signal)
# except:
# print ("cannot publish action")
# time.sleep(.5)
# return to initial joint positions
# rospy.wait_for_service('/gazebo/set_model_configuration')
# try:
# joint_names = ['base_to_femur_1', 'base_to_femur_2', 'base_to_femur_3', 'base_to_femur_4', 'femur_to_tibia_1', 'femur_to_tibia_2', 'femur_to_tibia_3', 'femur_to_tibia_4']
# joint_positions = [0, 0, 0, 0, 0, 0, 0, 0]
# self.set_model_config_proxy('spyndra', 'spyndra_description', joint_names, joint_positions)
# except (rospy.ServiceException) as e:
# print ("/gazebo/set_model_configuration service call failed")
# time.sleep(.5)
# return to initial model position
# rospy.wait_for_service('/gazebo/set_model_state')
# try:
# model_state = ModelState()
# model_state.model_name = 'spyndra'
# model_state.pose.position.x, model_state.pose.position.y, model_state.pose.position.z = 0, 0, 0.5
# model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, model_state.pose.orientation.w = 0, 0, 0, 0
# model_state.twist.linear.x, model_state.twist.linear.y, model_state.twist.linear.z = 0, 0, 0
# model_state.twist.angular.x, model_state.twist.angular.y, model_state.twist.angular.z = 0, 0, 0
# self.set_model_state_proxy(model_state)
# except (rospy.ServiceException) as e:
# print ("/gazebo/set_model_state service call failed")
# time.sleep(1)
# stand up
try:
# Wait for robot to be ready to accept signal
# rospy.wait_for_message('motor_state', MotorSignal, timeout=5)
motor_signal = MotorSignal()
motor_signal.motor_type = 1
# femeor 400~630
# tibia 650~900
position = [512, 512, 512, 512, 820, 820, 820, 820]
for i, p in enumerate(position):
self.control_node.pubish_signal(i, p)
except:
print ("cannot publish action")
time.sleep(10)
# initiate observation
s_ = np.zeros(self.nS)
# imu data update
# imu_data = None
# while imu_data is None:
# try:
# imu_data = rospy.wait_for_message('imu', Imu)
# s_[8: 14] = [imu_data.linear_acceleration.x, imu_data.linear_acceleration.y, imu_data.linear_acceleration.z, \
# imu_data.angular_velocity.x, imu_data.angular_velocity.y, imu_data.angular_velocity.z]
# except:
# pass
# motor data update
actual_pos = np.zeros(8)
while 0 in actual_pos:
try:
motor_data = rospy.wait_for_message('/motor_state', MotorSignal)
#s_[:8] = motor_data.signal
actual_pos[motor_data.motor_id] = motor_data.signal
except:
pass
# position data update
position_data = None
while position_data is None:
try:
position_data = rospy.wait_for_message('/gps/data', BeaconPos, timeout=5)
index = position_data.name.index("spyndra")
#s_[8: 11] =
x, y = position_data.x_m, position_data.y_m
self.INIT_POS = np.array([x, y])
self.GOAL = np.array(self.INIT_POS, copy=True)
self.GOAL[0] += 10
self.INIT_DIST = np.linalg.norm(self.INIT_POS - self.GOAL)
except:
pass
# rospy.wait_for_service('/gazebo/pause_physics')
# try:
# #resp_pause = pause.call()
# self.pause()
# except (rospy.ServiceException) as e:
# print ("/gazebo/pause_physics service call failed")
self.START_TIME = time.time()
return s_
def _step(self, action, s):
# take action and update the observation(state)
# rospy.wait_for_service('/gazebo/unpause_physics')
# try:
# self.unpause()
# except (rospy.ServiceException) as e:
# print ("/gazebo/unpause_physics service call failed")
s_ = s.copy()
# if the observation contains previous state
if len(s) > 8:
s_ = np.hstack((np.zeros(8), s_[ :-8]))
# update motor position
self.torque = 0.
try:
motor_signal = MotorSignal()
motor_signal.motor_type = 1
s_[:8] = s[:8] + np.array(action).round()
for i in range(4):
s_[i] = self.clip_signal(s_[i], 350, 630)
for i in range(4, 8, 1):
s_[i] = self.clip_signal(s_[i], 650, 890)
for i, p in enumerate(s_[:8]):
self.control_node.pubish_signal(i, p)
except:
print ("cannot publish action")
s_time = time.time()
# imu data update
# imu_data = None
# while imu_data is None:
# try:
# imu_data = rospy.wait_for_message('imu', Imu, timeout=5)
# s_[8: 14] = [imu_data.linear_acceleration.x, imu_data.linear_acceleration.y, imu_data.linear_acceleration.z, \
# imu_data.angular_velocity.x, imu_data.angular_velocity.y, imu_data.angular_velocity.z]
# except:
# pass
actual_pos = np.zeros(8)
#while (time.time() - s_time) < .1:
#motor_data = None
while 0 in actual_pos:
try:
motor_data = rospy.wait_for_message('/motor_state', MotorSignal, timeout=5)
actual_pos[motor_data.motor_id] = np.array(motor_data.signal)
except:
pass
# position data update
position_data = None
while position_data is None:
try:
position_data = rospy.wait_for_message('/gps/data', BeaconPos, timeout=5)
# index = position_data.name.index("spyndra")
position_data = [position_data.x_m, position_data.y_m]
#[position_data.pose[index].position.x, position_data.pose[index].position.y, position_data.pose[index].position.y]
except:
pass
# rospy.wait_for_service('/gazebo/pause_physics')
# try:
# self.pause()
# except (rospy.ServiceException) as e:
# print ("/gazebo/pause_physics service call failed")
# Reward Function
## Time reward
time_reward = 10 - .2 * (time.time() - self.START_TIME)
#time_reward = 0.
## Distance reward
curr_dist2goal = np.linalg.norm(np.array(position_data) - self.GOAL)
dist_reward = (self.INIT_DIST - curr_dist2goal) * 15. / self.INIT_DIST
#print "dist to goal:", curr_dist2goal, "dist reward:", dist_reward, "motor signal:", s_[:8]
## Angel reward
angl_reward = 0
#angl_reward = (2 * np.pi - angl2goal) * 15. / (2 * np.pi)
## Torque reward
torq_reward = -self.torque * 5
print(self.torque)
reward = time_reward + dist_reward * 5 + torq_reward + angl_reward
# threshold TBD
if curr_dist2goal < 1:
done = True
else:
done = False
return s_, reward, done, curr_dist2goal
def callback(data):
motor_signal = MotorSignal()
motor_signal.motor_type = 1
motor_signal.signal = [int(round((p * 195.569759) + 512)) for p in data.position]
motor_state_publisher.publish(motor_signal)