def dynamic(self, state):
command = AtlasSimInterfaceCommand()
command.behavior = AtlasSimInterfaceCommand.WALK
# k_effort is all 0s for full BDI controll of all joints.
command.k_effort = [0] * 28
# Observe next_step_index_needed to determine when to switch steps.
self.step_index = state.walk_feedback.next_step_index_needed
# A walk behavior command needs to know three additional steps beyond the current step needed to plan
# for the best balance
for i in range(4):
step_index = self.step_index + i
is_right_foot = step_index % 2
command.walk_params.step_queue[i].step_index = step_index
command.walk_params.step_queue[i].foot_index = is_right_foot
# A duration of 0.63s is a good default value
command.walk_params.step_queue[i].duration = 0.63
# As far as I can tell, swing_height has yet to be implemented
command.walk_params.step_queue[i].swing_height = 0.2
# Determine pose of the next step based on the step_index
command.walk_params.step_queue[i].pose = self.calculate_pose(
step_index)
# Publish this command every time we have a new state message
self.asi_command.publish(command)
def static(self, state):
# When the robot status_flags are 1 (SWAYING), you can publish the next step command.
if (state.step_feedback.status_flags == 1 and not self.is_swaying):
self.step_index += 1
self.is_swaying = True
print("Step " + str(self.step_index))
elif (state.step_feedback.status_flags == 2):
self.is_swaying = False
is_right_foot = self.step_index % 2
command = AtlasSimInterfaceCommand()
command.behavior = AtlasSimInterfaceCommand.STEP
# k_effort is all 0s for full bdi control of all joints
command.k_effort = [0] * 28
# step_index should always be one for a step command
command.step_params.desired_step.step_index = 1
command.step_params.desired_step.foot_index = is_right_foot
# duration has far as I can tell is not observed
command.step_params.desired_step.duration = 0.63
# swing_height is observed
command.step_params.desired_step.swing_height = 0.4
if self.step_index > 30:
print(str(self.calculate_pose(self.step_index)))
# Determine pose of the next step based on the number of steps we have taken
command.step_params.desired_step.pose = self.calculate_pose(self.step_index)
# Publish a new step command every time a state message is received
self.asi_command.publish(command)
def process_atlas(self, state):
self.state = state;
if not self._isWalking:
return
walk_goal = AtlasSimInterfaceCommand()
walk_goal.behavior = AtlasSimInterfaceCommand.WALK
walk_goal.walk_params.use_demo_walk = False
# 0 for full BDI control, 255 for PID control
walk_goal.k_effort = [0] * 28
# Load in the step array and pad it out
steps = self.steps + [ self.steps[-1] ] * 5
# Check if we have finished walking
if state.walk_feedback.current_step_index >= len(self.steps) - 2:
# Reset robot to standing
stand_goal = AtlasSimInterfaceCommand()
stand_goal.behavior = AtlasSimInterfaceCommand.STAND
self.command.publish(stand_goal)
# Clear step queue
self.steps = []
self._isWalking = False
rospy.loginfo('Completed walk!')
else:
# Send next steps to ATLAS
next_idx = state.walk_feedback.next_step_index_needed
walk_goal.walk_params.step_queue = steps[next_idx : next_idx + 4]
self.command.publish(walk_goal)
def Step(self):
if (3 < len(self._StepQueue)):
command = AtlasSimInterfaceCommand()
command.behavior = AtlasSimInterfaceCommand.WALK
for i in range(4):
command.walk_params.step_queue[i] = self._StepQueue[i]
else:
command = 0
# print("Sending STAND Command")
# command = AtlasSimInterfaceCommand(None,AtlasSimInterfaceCommand.STAND, None, None, None, None, [0]*28)
self._StepQueue.popleft()
self._index += 1
return command
def GetCommandStatic(self, state):
command = AtlasSimInterfaceCommand()
command.behavior = AtlasSimInterfaceCommand.STEP
#give user control over neck, back_z and arms
command.k_effort = self._k_effort
command.step_params.desired_step = self._LPP.GetNextStaticStep()
if (0 != command.step_params.desired_step):
# Not sure why such a magic number
command.step_params.desired_step.duration = 0.63
# Apparently this next line is a must
command.step_params.desired_step.step_index = 1
#command.step_params.desired_step = self._TransforFromGlobalToBDI(command.step_params.desired_step,state)
else:
command = 0
return command
def EmergencyStop(self):
#k_effort = [0] * 28
#k_effort[3] = 255
stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
None, None, None, None,
self._k_effort)
self.asi_command.publish(stand)
def _RequestFootPlacements(self):
print("Request Foot Placements")
# Perform a service request from FP
try:
# Handle preemption?
# if received a "End of mission" sort of message from FP
start_pose, other_foot_pose = self._GetStartingFootPose()
resp = self._foot_placement_client(0, start_pose, other_foot_pose)
if [] == resp.foot_placement_path: # 1 == resp.done:
self._WalkingModeStateMachine.PerformTransition("Finished")
else:
listSteps = []
for desired in resp.foot_placement_path:
#print("Desired: ",desired)
command = AtlasSimInterfaceCommand()
step = command.step_params.desired_step
step.foot_index = desired.foot_index
step.swing_height = desired.clearance_height
step.pose.position.x = desired.pose.position.x
step.pose.position.y = desired.pose.position.y
step.pose.position.z = desired.pose.position.z
Q = quaternion_from_euler(desired.pose.ang_euler.x,
desired.pose.ang_euler.y,
desired.pose.ang_euler.z)
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
listSteps.append(step)
self._LPP.SetPath(listSteps)
#print("listSteps: ",listSteps)
except rospy.ServiceException, e:
print "Foot Placement Service call failed: %s" % e
def harness():
# Initialize atlas mode and atlas_sim_interface_command publishers
_mode = rospy.Publisher('/atlas/mode', String, None, False, \
True, None)
_asi_command = rospy.Publisher('/atlas/atlas_sim_interface_command',
AtlasSimInterfaceCommand, None, False, True,
None)
rospy.sleep(2.0)
k_effort = [0] * 28
# Puts robot into freeze behavior, all joints controlled
freeze = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.FREEZE,
None, None, None, None, k_effort)
_asi_command.publish(freeze)
rospy.sleep(1.0)
# Puts robot into stand_prep behavior, a joint configuration suitable
# to go into stand mode
stand_prep = AtlasSimInterfaceCommand(None,
AtlasSimInterfaceCommand.STAND_PREP,
None, None, None, None, k_effort)
_asi_command.publish(stand_prep)
rospy.sleep(2.0)
_mode.publish("nominal")
# Put robot into stand position
stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
None, None, None, None, k_effort)
rospy.sleep(0.3)
_asi_command.publish(stand)
rospy.wait_for_service('gazebo/reset_models')
reset_gazebo_model = rospy.ServiceProxy('gazebo/reset_models', Empty)
reset_gazebo_model()
# Harness robot, with gravity off
_mode.publish("harnessed")
k_effort = [255] * 28
# Puts robot into freeze behavior, all joints controlled
freeze = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.USER,
None, None, None, None, k_effort)
_asi_command.publish(freeze)
def send_stand_command(self):
rospy.loginfo("set stand mode")
_sim_msg = AtlasSimInterfaceCommand(behavior=0,
k_effort=[
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0
])
_sim_msg.header.stamp = rospy.Time.now()
self.sim_command_pub.publish(_sim_msg)
def move(x, y, z, w=0):
# Initialize the node
rospy.init_node('move_atlas')
# Setup the publishers
mode = rospy.Publisher('/atlas/mode', String, None, False, True, None)
sim_interface = rospy.Publisher('/atlas/atlas_sim_interface_command',
AtlasSimInterfaceCommand, None, False,
True, None)
set_pose = rospy.Publisher('/atlas/set_pose', Pose, None, False, True,
None)
while set_pose.get_num_connections() == 0:
rospy.sleep(0.1)
while rospy.get_time() < 5.0:
rospy.sleep(0.1)
p = Pose()
p.position.x = x
p.position.y = y
p.position.z = z
p.orientation.x = 0
p.orientation.y = 0
p.orientation.z = sin(w / 2.0)
p.orientation.w = cos(w / 2.0)
com = AtlasSimInterfaceCommand()
rospy.sleep(0.1)
mode.publish("harnessed")
com.behavior = com.FREEZE
sim_interface.publish(com)
com.behavior = com.STAND_PREP
sim_interface.publish(com)
rospy.sleep(2.0)
mode.publish("nominal")
rospy.sleep(0.05)
set_pose.publish(p)
rospy.sleep(0.2)
com.behavior = com.STAND
sim_interface.publish(com)
def send_user_command(self):
rospy.loginfo("set user mode")
_sim_msg = AtlasSimInterfaceCommand(behavior=1,
k_effort=[
255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255,
255, 255, 255, 255, 255, 255,
255, 255, 255, 255
])
_sim_msg.header.stamp = rospy.Time.now()
self.sim_command_pub.publish(_sim_msg)
def GetCommandDynamic(self):
command = AtlasSimInterfaceCommand()
command.behavior = AtlasSimInterfaceCommand.WALK
# command.k_effort = [0] * 28
# k_effort = [0] * 28
# k_effort[3] = 255 # k_effort[0:4] = 4*[255]
# # k_effort[16:28] = 12*[255]
command.k_effort = self._k_effort
step_queue = self._LPP.GetNextDynamicStep()
if (0 == step_queue):
command = 0
else:
for i in range(4):
command.walk_params.step_queue[i] = copy.deepcopy(
step_queue[i])
command.walk_params.step_queue[
i].step_index = self._StepIndex + i
command.walk_params.step_queue[i].duration = 0.63
#print("GetCommand",command.walk_params.step_queue)
#command.walk_params.step_queue[i] = self._TransforFromGlobalToBDI(command.walk_params.step_queue[i],i)
return command
def EmergencyStop(self):
# Puts robot into freeze behavior, all joints controlled
# Put the robot into a known state
k_effort = [0] * 28
k_effort[3] = 255
# freeze = AtlasSimInterfaceCommand(None,AtlasSimInterfaceCommand.FREEZE, None, None, None, None, k_effort )
# self.asi_command.publish(freeze)
# rospy.sleep(0.3)
# Put robot into stand position
stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
None, None, None, None, k_effort)
def move(x, y, z, w=0):
# Initialize the node
rospy.init_node('move_atlas')
# Setup the publishers
mode = rospy.Publisher('/atlas/mode', String, None, False, True, None)
sim_interface = rospy.Publisher('/atlas/atlas_sim_interface_command', AtlasSimInterfaceCommand, None, False, True, None)
set_pose= rospy.Publisher('/atlas/set_pose', Pose, None, False, True, None)
while set_pose.get_num_connections() == 0:
rospy.sleep(0.1)
while rospy.get_time() < 5.0:
rospy.sleep(0.1)
p = Pose()
p.position.x = x
p.position.y = y
p.position.z = z
p.orientation.x = 0
p.orientation.y = 0
p.orientation.z = sin(w/2.0)
p.orientation.w = cos(w/2.0)
com = AtlasSimInterfaceCommand()
rospy.sleep(0.1)
mode.publish("harnessed")
com.behavior = com.FREEZE
sim_interface.publish(com)
com.behavior = com.STAND_PREP
sim_interface.publish(com)
rospy.sleep(2.0)
mode.publish("nominal")
rospy.sleep(0.05)
set_pose.publish(p)
rospy.sleep(0.2)
com.behavior = com.STAND
sim_interface.publish(com)
def _PrepareStepData(self):
self._index += 1
command = AtlasSimInterfaceCommand()
stepData = command.walk_params.step_queue[0]
stepData.step_index = self._index
stepData.foot_index = self._index % 2
stepData.duration = self._Duration
stepData.swing_height = self._SwingHeight
stepData.pose.position.z = 0.0
return stepData
def Initialize(self, parameters):
WalkingMode.Initialize(self, parameters)
self._Preview_Distance = 0.0 # planned ahead distance in BDI command
# Subscriber
self._Subscribers["Path"] = rospy.Subscriber('/path', C31_Waypoints,
self._path_cb)
self._Subscribers["Odometry"] = rospy.Subscriber(
'/C25/publish', C25C0_ROP, self._odom_cb) #
# self._Subscribers["Odometry"] = rospy.Subscriber('/ground_truth_odom',Odometry,self._odom_cb)
self._Subscribers["ASI_State"] = rospy.Subscriber(
'/atlas/atlas_sim_interface_state', AtlasSimInterfaceState,
self.asi_state_cb)
self._Subscribers["IMU"] = rospy.Subscriber('/atlas/imu', Imu,
self._get_imu)
self._Subscribers["JointStates"] = rospy.Subscriber(
'/atlas/joint_states', JointState, self._get_joints)
rospy.sleep(0.3)
self._bDone = False
# # Puts robot into freeze behavior, all joints controlled
# # Put the robot into a known state
k_effort = [0] * 28
k_effort[0:4] = 4 * [255]
k_effort[16:28] = 12 * [255]
self._JC.set_k_eff(k_effort)
self._JC.set_all_pos(self._cur_jnt)
self._JC.send_command()
# freeze = AtlasSimInterfaceCommand(None,AtlasSimInterfaceCommand.FREEZE, None, None, None, None, k_effort )
# self.asi_command.publish(freeze)
# # Puts robot into stand_prep behavior, a joint configuration suitable
# # to go into stand mode
# stand_prep = AtlasSimInterfaceCommand(None,AtlasSimInterfaceCommand.STAND_PREP, None, None, None, None, k_effort)
# self.asi_command.publish(stand_prep)
# rospy.sleep(2.0)
# self.mode.publish("nominal")
# Put robot into stand position
stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
None, None, None, None, k_effort)
rospy.sleep(0.3)
self.asi_command.publish(stand)
# Initialize some variables before starting.
self.is_swaying = False
self._BDI_StateMachine.Initialize(self.step_index_for_reset)
def Initialize(self, parameters):
WalkingMode.Initialize(self, parameters)
self._LPP.Initialize()
self._bRobotIsStatic = True
self._BDIswitch_client = rospy.ServiceProxy('C25/BDIswitch', C25BDI)
state = Int32()
state.data = 1
resp_switched_to_BDI_odom = self._BDIswitch_client(state)
print "Using BDI odom"
# rospy.wait_for_service('foot_placement_path') # used for clone
# self._foot_placement_client = rospy.ServiceProxy('foot_placement_path', FootPlacement_Service)
rospy.wait_for_service('foot_placement')
self._foot_placement_client = rospy.ServiceProxy(
'foot_placement', FootPlacement_Service)
# Subscribers:
self._Subscribers["Odometry"] = rospy.Subscriber(
'/C25/publish', C25C0_ROP, self._odom_cb)
self._Subscribers["ASI_State"] = rospy.Subscriber(
'/atlas/atlas_sim_interface_state', AtlasSimInterfaceState,
self.asi_state_cb)
self._Subscribers["IMU"] = rospy.Subscriber('/atlas/imu', Imu,
self._get_imu)
self._Subscribers["JointStates"] = rospy.Subscriber(
'/atlas/joint_states', JointState, self._get_joints)
rospy.sleep(0.3)
self._k_effort = [0] * 28
self._k_effort[3] = 255 # k_effort[0:4] = 4*[255]
# k_effort[16:28] = 12*[255]
self._JC.set_k_eff(self._k_effort)
self._JC.set_all_pos(self._cur_jnt)
self._JC.send_command()
self._RequestFootPlacements()
self._bDone = False
self._bIsSwaying = False
self._StepIndex = 1
self._command = 0
#self._bRobotIsStatic = False
self._GetOrientationDelta0Values(
) # Orientation difference between BDI odom and Global
# Put robot into stand position
stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
None, None, None, None,
self._k_effort)
self.asi_command.publish(stand)
rospy.sleep(0.3)
def Initialize(self, parameters):
WalkingMode.Initialize(self, parameters)
self._CD_StateMachine.Initialize(self.step_index_for_reset)
self._bDone = False
self._yaw = 0
self._BDIswitch_client = rospy.ServiceProxy('C25/BDIswitch', C25BDI)
state = Int32()
state.data = 1
resp_switched_to_BDI_odom = self._BDIswitch_client(state)
print "Using BDI odom"
# if resp_switched_to_BDI_odom:
# print "Using BDI odom"
# else:
# print "Using ROBIL odom"
# Subscriber
self._Subscribers["Path"] = rospy.Subscriber('/path', C31_Waypoints,
self._path_cb)
self._Subscribers["Odometry"] = rospy.Subscriber(
'/C25/publish', C25C0_ROP, self._odom_cb)
self._Subscribers["ASI_State"] = rospy.Subscriber(
'/atlas/atlas_sim_interface_state', AtlasSimInterfaceState,
self.asi_state_cb)
self._Subscribers["IMU"] = rospy.Subscriber('/atlas/imu', Imu,
self._get_imu)
self._Subscribers["JointStates"] = rospy.Subscriber(
'/atlas/joint_states', JointState, self._get_joints)
rospy.sleep(0.3)
self._k_effort = [0] * 28
self._k_effort[3] = 255
# self._k_effort[0:4] = 4*[255]
# self._k_effort[16:28] = 12*[255]
self._JC.set_k_eff(self._k_effort)
self._JC.set_all_pos(self._cur_jnt)
self._JC.send_command()
# Put robot into stand position
stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
None, None, None, None,
self._k_effort)
self.asi_command.publish(stand)
rospy.sleep(0.3)
def Initialize(self, parameters):
WalkingMode.Initialize(self, parameters)
self._command = 0
self._bRobotIsStatic = True
rospy.wait_for_service('foot_placement_path')
self._foot_placement_client = rospy.ServiceProxy(
'foot_placement_path', FootPlacement_Service)
# Subscribers:
self._Subscribers["Odometry"] = rospy.Subscriber(
'/C25/publish', C25C0_ROP, self._odom_cb)
self._Subscribers["ASI_State"] = rospy.Subscriber(
'/atlas/atlas_sim_interface_state', AtlasSimInterfaceState,
self.asi_state_cb)
self._Subscribers["IMU"] = rospy.Subscriber('/atlas/imu', Imu,
self._get_imu)
self._Subscribers["JointStates"] = rospy.Subscriber(
'/atlas/joint_states', JointState, self._get_joints)
rospy.sleep(0.3)
k_effort = [0] * 28
k_effort[0:4] = 4 * [255]
k_effort[16:28] = 12 * [255]
self._JC.set_k_eff(k_effort)
self._JC.set_all_pos(self._cur_jnt)
self._JC.send_command()
self._bDone = False
self._bIsSwaying = False
self._bRobotIsStatic = False
self._RequestFootPlacements()
self._GetOrientationDelta0Values(
) # Orientation difference between BDI odom and Global
# Put robot into stand position
stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
None, None, None, None,
self._k_effort)
self.asi_command.publish(stand)
rospy.sleep(0.3)
def demo(self):
# Step 1: Go to stand-prep pose under user control
stand_prep_msg = AtlasCommand()
# Always insert current time
stand_prep_msg.header.stamp = rospy.Time.now()
# Assign some position and gain values that will get us there.
stand_prep_msg.position = [
2.438504816382192e-05, 0.0015186156379058957,
9.983908967114985e-06, -0.0010675729718059301,
-0.0003740221436601132, 0.06201673671603203, -0.2333149015903473,
0.5181407332420349, -0.27610817551612854, -0.062101610004901886,
0.00035181696875952184, -0.06218484416604042, -0.2332201600074768,
0.51811283826828, -0.2762000858783722, 0.06211360543966293,
0.29983898997306824, -1.303462266921997, 2.0007927417755127,
0.49823325872421265, 0.0003098883025813848, -0.0044272784143686295,
0.29982614517211914, 1.3034454584121704, 2.000779867172241,
-0.498238742351532, 0.0003156556049361825, 0.004448802210390568
]
stand_prep_msg.velocity = [0.0] * self.NUM_JOINTS
stand_prep_msg.effort = [0.0] * self.NUM_JOINTS
stand_prep_msg.k_effort = [255] * self.NUM_JOINTS
# Publish and give time to take effect
print('[USER] Going to stand prep position...')
self.ac_pub.publish(stand_prep_msg)
time.sleep(2.0)
# Step 2: Request BDI stand mode
stand_msg = AtlasSimInterfaceCommand()
# Always insert current time
stand_msg.header.stamp = rospy.Time.now()
# Tell it to stand
stand_msg.behavior = stand_msg.STAND_PREP
# Set k_effort = [255] to indicate that we still want all joints under user
# control. The stand behavior needs a few iterations before it start
# outputting force values.
stand_msg.k_effort = [255] * self.NUM_JOINTS
# Publish and give time to take effect
print('[USER] Warming up BDI stand...')
self.asic_pub.publish(stand_msg)
time.sleep(1.0)
# Now switch to stand
stand_msg.behavior = stand_msg.STAND
# Set k_effort = [0] to indicate that we want all joints under BDI control
stand_msg.k_effort = [0] * self.NUM_JOINTS
# Publish and give time to take effect
print('[BDI] Standing...')
self.asic_pub.publish(stand_msg)
time.sleep(2.0)
# Step 3: Move the arms and head a little (not too much; don't want to fall
# over)
slight_movement_msg = AtlasCommand()
# Always insert current time
slight_movement_msg.header.stamp = rospy.Time.now()
# Start with 0.0 and set values for the joints that we want to control
slight_movement_msg.position = [0.0] * self.NUM_JOINTS
slight_movement_msg.position[AtlasState.neck_ry] = -0.1
slight_movement_msg.position[AtlasState.l_arm_ely] = 2.1
slight_movement_msg.position[AtlasState.l_arm_wrx] = -0.1
slight_movement_msg.position[AtlasState.r_arm_ely] = 2.1
slight_movement_msg.position[AtlasState.r_arm_wrx] = -0.1
slight_movement_msg.velocity = [0.0] * self.NUM_JOINTS
slight_movement_msg.effort = [0.0] * self.NUM_JOINTS
slight_movement_msg.kp_position = [
20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0,
300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0,
200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0,
100.0
]
slight_movement_msg.ki_position = [0.0] * self.NUM_JOINTS
slight_movement_msg.kd_position = [0.0] * self.NUM_JOINTS
# Bump up kp_velocity to reduce the jerkiness of the transition
stand_prep_msg.kp_velocity = [50.0] * self.NUM_JOINTS
slight_movement_msg.i_effort_min = [0.0] * self.NUM_JOINTS
slight_movement_msg.i_effort_max = [0.0] * self.NUM_JOINTS
# Set k_effort = [1] for the joints that we want to control.
# BDI has control of the other joints
slight_movement_msg.k_effort = [0] * self.NUM_JOINTS
slight_movement_msg.k_effort[AtlasState.neck_ry] = 255
slight_movement_msg.k_effort[AtlasState.l_arm_ely] = 255
slight_movement_msg.k_effort[AtlasState.l_arm_wrx] = 255
slight_movement_msg.k_effort[AtlasState.r_arm_ely] = 255
slight_movement_msg.k_effort[AtlasState.r_arm_wrx] = 255
# Publish and give time to take effect
print('[USER/BDI] Command neck and arms...')
self.ac_pub.publish(slight_movement_msg)
time.sleep(2.0)
# Step 4: Request BDI walk
walk_msg = AtlasSimInterfaceCommand()
# Always insert current time
walk_msg.header.stamp = rospy.Time.now()
# Tell it to walk
walk_msg.behavior = walk_msg.WALK
walk_msg.walk_params.use_demo_walk = False
# Fill in some steps
for i in range(4):
step_data = AtlasBehaviorStepData()
# Steps are indexed starting at 1
step_data.step_index = i + 1
# 0 = left, 1 = right
step_data.foot_index = i % 2
# 0.3 is a good number
step_data.swing_height = 0.3
# 0.63 is a good number
step_data.duration = 0.63
# We'll specify desired foot poses in ego-centric frame then
# transform them into the robot's world frame.
# Match feet so that we end with them together
step_data.pose.position.x = (1 + i / 2) * 0.25
# Step 0.15m to either side of center, alternating with feet
step_data.pose.position.y = 0.15 if (i % 2 == 0) else -0.15
step_data.pose.position.z = 0.0
# Point those feet straight ahead
step_data.pose.orientation.x = 0.0
step_data.pose.orientation.y = 0.0
step_data.pose.orientation.z = 0.0
step_data.pose.orientation.w = 1.0
# Transform this foot pose according to robot's
# current estimated pose in the world, which is a combination of IMU
# and internal position estimation.
# http://www.ros.org/wiki/kdl/Tutorials/Frame%20transformations%20%28Python%29
f1 = posemath.fromMsg(step_data.pose)
f2 = PyKDL.Frame(
PyKDL.Rotation.Quaternion(self.imu_msg.orientation.x,
self.imu_msg.orientation.y,
self.imu_msg.orientation.z,
self.imu_msg.orientation.w),
PyKDL.Vector(self.asis_msg.pos_est.position.x,
self.asis_msg.pos_est.position.y,
self.asis_msg.pos_est.position.z))
f = f2 * f1
step_data.pose = posemath.toMsg(f)
walk_msg.walk_params.step_queue[i] = step_data
# Use the same k_effort from the last step, to retain user control over some
# joints. BDI has control of the other joints.
walk_msg.k_effort = slight_movement_msg.k_effort
# Publish and give time to take effect
print('[USER/BDI] Walking...')
self.asic_pub.publish(walk_msg)
time.sleep(6.0)
# Step 5: Go back to home pose under user control
home_msg = AtlasCommand()
# Always insert current time
home_msg.header.stamp = rospy.Time.now()
# Assign some position and gain values that will get us there.
home_msg.position = [0.0] * self.NUM_JOINTS
home_msg.velocity = [0.0] * self.NUM_JOINTS
home_msg.effort = [0.0] * self.NUM_JOINTS
home_msg.kp_position = [
20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0,
300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0,
200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0,
100.0
]
home_msg.ki_position = [0.0] * self.NUM_JOINTS
home_msg.kd_position = [0.0] * self.NUM_JOINTS
# Bump up kp_velocity to reduce the jerkiness of the transition
home_msg.kp_velocity = [50.0] * self.NUM_JOINTS
home_msg.i_effort_min = [0.0] * self.NUM_JOINTS
home_msg.i_effort_max = [0.0] * self.NUM_JOINTS
# Set k_effort = [1] to indicate that we want all joints under user control
home_msg.k_effort = [255] * self.NUM_JOINTS
# Publish and give time to take effect
print('[USER] Going to home position...')
self.ac_pub.publish(home_msg)
time.sleep(2.0)
def _switch_mode_to_manual(self):
message = AtlasSimInterfaceCommand()
message.header.stamp = rospy.Time.now()
message.behavior = AtlasSimInterfaceCommand.USER
self._atlas_sim_interface_command_publisher.publish(message)
time.sleep(0.5)
def GetCommand(self):
command = AtlasSimInterfaceCommand()
command.behavior = AtlasSimInterfaceCommand.WALK
for i in range(4):
command.walk_params.step_queue[i] = self._StepQueue.Peek(i)
return command
'r_arm_ely',
'r_arm_elx',
'r_arm_uwy',
'r_arm_mwx'
] #27
_JC = JointCommands_msg_handler(robot_name, jnt_names)
_k_effort = [0] * 28
#self._k_effort[3] = 255
_k_effort[3:4] = 1 * [255]
_k_effort[16:28] = 12 * [255]
_JC.set_k_eff(_k_effort)
_JC.set_all_pos(_cur_jnt)
_JC.send_command()
# Put robot into stand position
stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
None, None, None, None, _k_effort)
asi_command.publish(stand)
rospy.sleep(0.3)
print "Changed k_efforts"
#
#class CD_WalkingMode(WalkingMode):
#
#class CD_WalkingMode(WalkingMode):
# def __init__(self):
# WalkingMode.__init__(self,CD_PathPlanner())
# self.step_index_for_reset = 0
# # Initialize atlas mode and atlas_sim_interface_command publishers
#
#
# self._CD_StateMachine = CD_StateMachine(self._LPP)
def demo(self):
# Step 0: Go to home pose under user control
home_msg = AtlasCommand()
# Always insert current time
home_msg.header.stamp = rospy.Time.now()
# Assign some position and gain values that will get us there.
home_msg.position = [0.0] * self.NUM_JOINTS
home_msg.velocity = [0.0] * self.NUM_JOINTS
home_msg.effort = [0.0] * self.NUM_JOINTS
home_msg.kp_position = [20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0]
home_msg.ki_position = [0.0] * self.NUM_JOINTS
home_msg.kd_position = [0.0] * self.NUM_JOINTS
# Bump up kp_velocity to reduce the jerkiness of the transition
home_msg.kp_velocity = [50.0] * self.NUM_JOINTS
home_msg.i_effort_min = [0.0] * self.NUM_JOINTS
home_msg.i_effort_max = [0.0] * self.NUM_JOINTS
# Set k_effort = [1] to indicate that we want all joints under user control
home_msg.k_effort = [255] * self.NUM_JOINTS
# Publish and give time to take effect
print('[USER] Going to home position...')
self.ac_pub.publish(home_msg)
time.sleep(2.0)
# Step 1: Go to stand-prep pose under user control
stand_prep_msg = AtlasCommand()
# Always insert current time
stand_prep_msg.header.stamp = rospy.Time.now()
# Assign some position and gain values that will get us there.
stand_prep_msg.position = [2.438504816382192e-05, 0.0015186156379058957, 9.983908967114985e-06, -0.0010675729718059301, -0.0003740221436601132, 0.06201673671603203, -0.2333149015903473, 0.5181407332420349, -0.27610817551612854, -0.062101610004901886, 0.00035181696875952184, -0.06218484416604042, -0.2332201600074768, 0.51811283826828, -0.2762000858783722, 0.06211360543966293, 0.29983898997306824, -1.303462266921997, 2.0007927417755127, 0.49823325872421265, 0.0003098883025813848, -0.0044272784143686295, 0.29982614517211914, 1.3034454584121704, 2.000779867172241, -0.498238742351532, 0.0003156556049361825, 0.004448802210390568]
stand_prep_msg.velocity = [0.0] * self.NUM_JOINTS
stand_prep_msg.effort = [0.0] * self.NUM_JOINTS
stand_prep_msg.kp_position = [20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0]
stand_prep_msg.ki_position = [0.0] * self.NUM_JOINTS
stand_prep_msg.kd_position = [0.0] * self.NUM_JOINTS
# Bump up kp_velocity to reduce the jerkiness of the transition
stand_prep_msg.kp_velocity = [50.0] * self.NUM_JOINTS
stand_prep_msg.i_effort_min = [0.0] * self.NUM_JOINTS
stand_prep_msg.i_effort_max = [0.0] * self.NUM_JOINTS
# Set k_effort = [1] to indicate that we want all joints under user control
stand_prep_msg.k_effort = [255] * self.NUM_JOINTS
# Publish and give time to take effect
print('[USER] Going to stand prep position...')
self.ac_pub.publish(stand_prep_msg)
time.sleep(2.0)
# Step 2: Request BDI stand mode
stand_msg = AtlasSimInterfaceCommand()
# Always insert current time
stand_msg.header.stamp = rospy.Time.now()
# Tell it to stand
stand_msg.behavior = stand_msg.STAND_PREP
# Set k_effort = [255] to indicate that we still want all joints under user
# control. The stand behavior needs a few iterations before it start
# outputting force values.
stand_msg.k_effort = [255] * self.NUM_JOINTS
# Publish and give time to take effect
print('[USER] Warming up BDI stand...')
self.asic_pub.publish(stand_msg)
time.sleep(1.0)
# Now switch to stand
stand_msg.behavior = stand_msg.STAND
# Set k_effort = [0] to indicate that we want all joints under BDI control
stand_msg.k_effort = [0] * self.NUM_JOINTS
# Publish and give time to take effect
print('[BDI] Standing...')
self.asic_pub.publish(stand_msg)
time.sleep(2.0)
# Step 3: Move the arms and head a little (not too much; don't want to fall
# over)
slight_movement_msg = AtlasCommand()
# Always insert current time
slight_movement_msg.header.stamp = rospy.Time.now()
# Start with 0.0 and set values for the joints that we want to control
slight_movement_msg.position = [0.0] * self.NUM_JOINTS
slight_movement_msg.position[AtlasState.neck_ay] = -0.1
slight_movement_msg.position[AtlasState.l_arm_ely] = 2.1
slight_movement_msg.position[AtlasState.l_arm_mwx] = -0.1
slight_movement_msg.position[AtlasState.r_arm_ely] = 2.1
slight_movement_msg.position[AtlasState.r_arm_mwx] = -0.1
slight_movement_msg.velocity = [0.0] * self.NUM_JOINTS
slight_movement_msg.effort = [0.0] * self.NUM_JOINTS
slight_movement_msg.kp_position = [20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0]
slight_movement_msg.ki_position = [0.0] * self.NUM_JOINTS
slight_movement_msg.kd_position = [0.0] * self.NUM_JOINTS
# Bump up kp_velocity to reduce the jerkiness of the transition
stand_prep_msg.kp_velocity = [50.0] * self.NUM_JOINTS
slight_movement_msg.i_effort_min = [0.0] * self.NUM_JOINTS
slight_movement_msg.i_effort_max = [0.0] * self.NUM_JOINTS
# Set k_effort = [1] for the joints that we want to control.
# BDI has control of the other joints
slight_movement_msg.k_effort = [0] * self.NUM_JOINTS
slight_movement_msg.k_effort[AtlasState.neck_ay] = 255
slight_movement_msg.k_effort[AtlasState.l_arm_ely] = 255
slight_movement_msg.k_effort[AtlasState.l_arm_mwx] = 255
slight_movement_msg.k_effort[AtlasState.r_arm_ely] = 255
slight_movement_msg.k_effort[AtlasState.r_arm_mwx] = 255
# Publish and give time to take effect
print('[USER/BDI] Command neck and arms...')
self.ac_pub.publish(slight_movement_msg)
time.sleep(2.0)
# Step 4: Request BDI walk
walk_msg = AtlasSimInterfaceCommand()
# Always insert current time
walk_msg.header.stamp = rospy.Time.now()
# Tell it to walk
walk_msg.behavior = walk_msg.WALK
walk_msg.walk_params.use_demo_walk = False
# Fill in some steps
for i in range(4):
step_data = AtlasBehaviorStepData()
# Steps are indexed starting at 1
step_data.step_index = i+1
# 0 = left, 1 = right
step_data.foot_index = i%2
# 0.3 is a good number
step_data.swing_height = 0.3
# 0.63 is a good number
step_data.duration = 0.63
# We'll specify desired foot poses in ego-centric frame then
# transform them into the robot's world frame.
# Match feet so that we end with them together
step_data.pose.position.x = (1+i/2)*0.25
# Step 0.15m to either side of center, alternating with feet
step_data.pose.position.y = 0.15 if (i%2==0) else -0.15
step_data.pose.position.z = 0.0
# Point those feet straight ahead
step_data.pose.orientation.x = 0.0
step_data.pose.orientation.y = 0.0
step_data.pose.orientation.z = 0.0
step_data.pose.orientation.w = 1.0
# Transform this foot pose according to robot's
# current estimated pose in the world, which is a combination of IMU
# and internal position estimation.
# http://www.ros.org/wiki/kdl/Tutorials/Frame%20transformations%20%28Python%29
f1 = posemath.fromMsg(step_data.pose)
f2 = PyKDL.Frame(PyKDL.Rotation.Quaternion(self.imu_msg.orientation.x,
self.imu_msg.orientation.y,
self.imu_msg.orientation.z,
self.imu_msg.orientation.w),
PyKDL.Vector(self.asis_msg.pos_est.position.x,
self.asis_msg.pos_est.position.y,
self.asis_msg.pos_est.position.z))
f = f2 * f1
step_data.pose = posemath.toMsg(f)
walk_msg.walk_params.step_queue[i] = step_data
# Use the same k_effort from the last step, to retain user control over some
# joints. BDI has control of the other joints.
walk_msg.k_effort = slight_movement_msg.k_effort
# Publish and give time to take effect
print('[USER/BDI] Walking...')
self.asic_pub.publish(walk_msg)
time.sleep(6.0)
# Step 5: Go back to home pose under user control
home_msg = AtlasCommand()
# Always insert current time
home_msg.header.stamp = rospy.Time.now()
# Assign some position and gain values that will get us there.
home_msg.position = [0.0] * self.NUM_JOINTS
home_msg.velocity = [0.0] * self.NUM_JOINTS
home_msg.effort = [0.0] * self.NUM_JOINTS
home_msg.kp_position = [20.0, 4000.0, 2000.0, 20.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 5.0, 100.0, 2000.0, 1000.0, 900.0, 300.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0, 2000.0, 1000.0, 200.0, 200.0, 50.0, 100.0]
home_msg.ki_position = [0.0] * self.NUM_JOINTS
home_msg.kd_position = [0.0] * self.NUM_JOINTS
# Bump up kp_velocity to reduce the jerkiness of the transition
home_msg.kp_velocity = [50.0] * self.NUM_JOINTS
home_msg.i_effort_min = [0.0] * self.NUM_JOINTS
home_msg.i_effort_max = [0.0] * self.NUM_JOINTS
# Set k_effort = [1] to indicate that we want all joints under user control
home_msg.k_effort = [255] * self.NUM_JOINTS
# Publish and give time to take effect
print('[USER] Going to home position...')
self.ac_pub.publish(home_msg)
time.sleep(2.0)
def _RequestTargetPose(self, desired_object):
# Perform a service request from FP
try:
# Handle preemption?
# if received a "End of mission" sort of message from FP
#start_pose,other_foot_pose = self._GetStartingFootPose()
if None == self._target_pose:
print("Request Target Pose to ", desired_object)
self._target_pose = self._foot_placement_client(
desired_object) # "target: 'Firehose'") #
print("Got from C23/C66 service the following Pose:",
self._target_pose)
delta_yaw, delta_trans = self._GetDeltaToObject(
desired_object, self._target_pose)
elif "Rotate_Translate_delta" == self._target_pose and not self._started_to_walk:
delta_yaw = self._delta_yaw
delta_trans = self._delta_trans
else:
delta_yaw = 0.0
delta_trans = Point()
delta_trans.x = 0.0
delta_trans.y = 0.0
delta_trans.z = 0.0
start_position = copy.copy(self._BDI_Static_pose.position)
start_orientation = euler_from_quaternion([
self._BDI_Static_pose.orientation.x,
self._BDI_Static_pose.orientation.y,
self._BDI_Static_pose.orientation.z,
self._BDI_Static_pose.orientation.w
])
self._foot_placement_path = []
if math.fabs(delta_yaw) > self._err_rot:
self._foot_placement_path = self._foot_placement_path + self._GetRotationDeltaFP_Path(
delta_yaw, start_position, start_orientation)
self._started_to_walk = True
if self._DistanceXY(delta_trans) > self._err_trans:
self._foot_placement_path = self._foot_placement_path + self._GetTranslationDeltaFP_Path(
delta_yaw, delta_trans, start_position, start_orientation)
self._started_to_walk = True
#listSteps = []
# if (math.fabs(delta_yaw) <= self._err_rot) and (self._DistanceXY(delta_trans) <= self._err_trans): # finished task
if [] == self._foot_placement_path and self._started_to_walk: # 1 == resp.done:
self._WalkingModeStateMachine.PerformTransition("Finished")
# if big error need to finish with error (didn't reach goal)
else:
if not self._started_to_walk:
self._started_to_walk = True
## Step in place: two first steps
self._foot_placement_path = self._GetTwoFirstStepFP_Path(
start_position, start_orientation, False)
listSteps = []
for desired in self._foot_placement_path:
command = AtlasSimInterfaceCommand()
step = command.step_params.desired_step
step.foot_index = desired.foot_index
step.swing_height = desired.clearance_height
step.pose.position.x = desired.pose.position.x
step.pose.position.y = desired.pose.position.y
step.pose.position.z = desired.pose.position.z
Q = quaternion_from_euler(desired.pose.ang_euler.x,
desired.pose.ang_euler.y,
desired.pose.ang_euler.z)
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
#print ("step command:",step)
listSteps.append(step)
self._LPP.SetPath(listSteps)
#print(listSteps)
except rospy.ServiceException, e:
print "Foot Placement Service call failed: %s" % e
def Initialize(self, parameters):
WalkingMode.Initialize(self, parameters)
self._LPP.Initialize()
self._command = 0
self._bRobotIsStatic = True
self._bGotStaticPose = False
self._BDI_Static_pose = Pose()
self._started_to_walk = False
self._target_pose = None
self._StepIndex = 1
self._isDynamic = False
self._stepWidth = 0.25 # Width of stride
self._theta_max = 0.30 # max turning angle per step
self._x_length_max = 0.25 # [meters] max step length (radius =~ 0.42 [m])
self._y_length_max = 0.15 # [meters]
# parameters to tune (see also 'Motion' task parameters):
self._err_rot = 0.018 #0.10 # [rad]
self._err_trans = 0.02 # 0.1 # [meters]
## USING task parameters:
if ((None != parameters) and ('Motion' in parameters)):
DesiredMotion = parameters['Motion']
if "Dynamic" == DesiredMotion: # Dynamic parameters
self._isDynamic = True
self._stepWidth = 0.2 #0.3 # Width of stride
self._theta_max = 0.15 #0.35 # max turning angle per step
self._x_length_max = 0.02 #0.25 # [meters] max step length (radius =~ 0.42 [m])
self._y_length_max = 0.15 #0.2 # [meters]
if "Dynamic10" == DesiredMotion: # Dynamic parameters
self._isDynamic = True
self._stepWidth = 0.2 #0.3 # Width of stride
self._theta_max = 0.15 #0.35 # max turning angle per step
self._x_length_max = 0.10 #0.25 # [meters] max step length (radius =~ 0.42 [m])
self._y_length_max = 0.10 #0.2 # [meters]
else: # Quasi-Static parameters (default)
self._isDynamic = False
self._stepWidth = 0.25 # Width of stride
self._theta_max = 0.30 # max turning angle per step
self._x_length_max = 0.25 # [meters] max step length (radius =~ 0.42 [m])
self._y_length_max = 0.15 # [meters]
self._R = self._stepWidth / 2 # Radius of turn, turn in place
# parameter 'Object' uses service to get delta alignment to target object
if ((None != parameters) and ('Object' in parameters)):
self._DesiredObject = parameters['Object']
else:
self._DesiredObject = "delta"
# parameter to turn in place and move (translate):
self._delta_yaw = 0.0
self._delta_trans = Point()
self._delta_trans.x = 0.0
self._delta_trans.y = 0.0
self._delta_trans.z = 0.0
# 'turn_in_place_Yaw' uses the parameter of the Yaw angle to turn in place
if ((None != parameters) and ('turn_in_place_Yaw' in parameters)):
self._target_pose = "Rotate_Translate_delta"
self._delta_yaw = float(parameters['turn_in_place_Yaw'])
# 'Xmovement' uses the parameter to translate x meters (+=fwd/-=bwd) from the starting place of the robot
if ((None != parameters) and ('Xmovement' in parameters)):
self._target_pose = "Rotate_Translate_delta"
self._delta_trans.x = float(parameters['Xmovement'])
# 'Ymovement' uses the parameter to translate y meters (+=left/-=right) from the starting place of the robot
if ((None != parameters) and ('Ymovement' in parameters)):
self._target_pose = "Rotate_Translate_delta"
self._delta_trans.y = float(parameters['Ymovement'])
if None == self._target_pose:
rospy.wait_for_service('C23/C66')
self._foot_placement_client = rospy.ServiceProxy(
'C23/C66', C23_orient) # object recognition service
# rospy.wait_for_service('foot_aline_pose')
# self._foot_placement_client = rospy.ServiceProxy('foot_aline_pose', C23_orient) # clone_service
self._BDIswitch_client = rospy.ServiceProxy('C25/BDIswitch', C25BDI)
state = Int32()
state.data = 1
resp_switched_to_BDI_odom = self._BDIswitch_client(state)
print "Using BDI odom"
# Subscribers:
#self._Subscribers["Odometry"] = rospy.Subscriber('/ground_truth_odom',Odometry,self._odom_cb)
self._Subscribers["ASI_State"] = rospy.Subscriber(
'/atlas/atlas_sim_interface_state', AtlasSimInterfaceState,
self.asi_state_cb)
self._Subscribers["IMU"] = rospy.Subscriber('/atlas/imu', Imu,
self._get_imu)
self._Subscribers["JointStates"] = rospy.Subscriber(
'/atlas/joint_states', JointState, self._get_joints)
rospy.sleep(0.3)
self._RequestTargetPose(self._DesiredObject)
self._k_effort = [0] * 28
self._k_effort[3] = 255
# self._k_effort[0:4] = 4*[255]
# self._k_effort[16:28] = 12*[255]
self._JC.set_k_eff(self._k_effort)
self._JC.set_all_pos(self._cur_jnt)
self._JC.send_command()
self._bDone = False
self._bIsSwaying = False
#self._bRobotIsStatic = False
#self._GetOrientationDelta0Values() # Orientation difference between BDI odom and Global
# Put robot into stand position
stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
None, None, None, None,
self._k_effort)
self.asi_command.publish(stand)
rospy.sleep(0.3)
