def load_position(self):
self.steps = []
step = AtlasBehaviorStepData()
step.step_index = self.state.walk_feedback.current_step_index
step.pose = self.state.foot_pos_est[0]
self.steps.append(step)
def add_step(self, radius):
L = self.params["Forward Stride Length"]["value"]
W = self.params["Stride Width"]["value"]
T = self.params["In Place Turn Size"]["value"]
R = radius
# Get origin based on last step
if not self.steps:
raise Exception("Unknown foot positions.")
last_step = self.steps[-1]
rpy = euler_from_quaternion([
last_step.pose.orientation.x,
last_step.pose.orientation.y,
last_step.pose.orientation.z,
last_step.pose.orientation.w,
])
Th = rpy[2]
X = last_step.pose.position.x + signum(last_step.foot_index) * W/2 * math.sin(Th)
Y = last_step.pose.position.y - signum(last_step.foot_index) * W/2 * math.cos(Th)
# Transform from here to next step center
if (R > L):
dTh = math.asin(L / R)
else:
dTh = signum(R) * T
dX = R * math.sin(dTh)
dY = R * (1 - math.cos(dTh))
# Transform to next foot position
dX = dX + signum(last_step.foot_index) * W/2 * math.sin(dTh)
dY = dY - signum(last_step.foot_index) * W/2 * math.cos(dTh)
# Store this foot position
Q = quaternion_from_euler(0, 0, Th + dTh)
step = AtlasBehaviorStepData()
step.step_index = last_step.step_index + 1
step.foot_index = 1 if (last_step.foot_index == 0) else 0
step.duration = self.params["Stride Duration"]["value"]
step.pose.position.x = X + math.cos(Th)*dX - math.sin(Th)*dY
step.pose.position.y = Y + math.sin(Th)*dX + math.cos(Th)*dY
step.pose.position.z = self.params["Step Height"]["value"]
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = self.params["Swing Height"]["value"]
self.steps.append(step)
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 twist(self, forward, lateral, turn):
steps = []
L = self.params["Forward Stride Length"]["value"]
L_lat = self.params["Lateral Stride Length"]["value"]
R = self.params["Turn Radius"]["value"]
W = self.params["Stride Width"]["value"]
X = 0
Y = 0
theta = 0
dTheta = 0
if forward != 0:
dTheta = turn * 2 * math.asin(L / (2 * (R + self.params["Stride Width"]["value"] / 2)))
else:
dTheta = turn * self.params["In Place Turn Size"]["value"]
steps = []
# This home step doesn't currently do anything, but it's a
# response to bdi not visiting the first step in a trajectory
home_step = AtlasBehaviorStepData()
# If moving right, first dummy step is on the left
home_step.foot_index = 1 * (lateral < 0)
home_step.pose.position.y = 0.1
steps.append(home_step)
prevX = 0
prevY = 0
# Builds the sequence of steps needed
for i in range(self.params["Walk Sequence Length"]["value"]):
theta += (turn != 0) * dTheta
# is_right_foot = 1, when stepping with right
is_even = i % 2
is_odd = 1 - is_even
is_right_foot = is_even
is_left_foot = is_odd
# left = 1, right = -1
foot = 1 - 2 * is_right_foot
if turn == 0:
X = (forward != 0) * (X + forward * L)
Y = (lateral != 0) * (Y + is_odd * lateral * L_lat) + foot * W / 2
elif forward != 0:
# Radius from point to foot (if turning)
R_foot = R + foot * W / 2
# turn > 0 for CCW, turn < 0 for CW
X = forward * turn * R_foot * math.sin(theta)
Y = forward * turn * (R - R_foot * math.cos(theta))
self.debuginfo(
"R: "
+ str(R)
+ " R_foot:"
+ str(R_foot)
+ " theta: "
+ str(theta)
+ " math.sin(theta): "
+ str(math.sin(theta))
+ " math.cos(theta) + "
+ str(math.cos(theta))
)
elif turn != 0:
X = turn * W / 2 * math.sin(theta)
Y = turn * W / 2 * math.cos(theta)
Q = quaternion_from_euler(0, 0, theta)
step = AtlasBehaviorStepData()
# One step already exists, so add one to index
step.step_index = i + 1
# Alternate between feet, start with left
step.foot_index = is_right_foot
# If moving laterally to the left, start with the right foot
if lateral > 0:
step.foot_index = is_left_foot
step.duration = self.params["Stride Duration"]["value"]
step.pose.position.x = X
step.pose.position.y = Y
step.pose.position.z = self.params["Step Height"]["value"]
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = self.params["Swing Height"]["value"]
steps.append(step)
# Add final step to bring feet together
is_right_foot = 1 - steps[-1].foot_index
is_even = is_right_foot
# foot = 1 for left, foot = -1 for right
foot = 1 - 2 * is_right_foot
if turn == 0:
Y = Y - foot * W
elif forward != 0:
self.debuginfo(
"R: "
+ str(R)
+ " R_foot:"
+ str(R_foot)
+ " theta: "
+ str(theta)
+ " math.sin(theta): "
+ str(math.sin(theta))
+ " math.cos(theta) + "
+ str(math.cos(theta))
)
# R_foot is radius to foot
R_foot = R + foot * W / 2
# turn > 0 for counter clockwise
X = forward * turn * R_foot * math.sin(theta)
Y = forward * turn * (R - R_foot * math.cos(theta))
else:
X = turn * W / 2 * math.sin(theta)
Y = turn * W / 2 * math.cos(theta)
Q = quaternion_from_euler(0, 0, theta)
step = AtlasBehaviorStepData()
step.step_index = len(steps)
step.foot_index = is_right_foot
step.duration = self.params["Stride Duration"]["value"]
step.pose.position.x = X
step.pose.position.y = Y
step.pose.position.z = self.params["Step Height"]["value"]
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = self.params["Swing Height"]["value"]
steps.append(step)
# 0 for full BDI control, 255 for PID control
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]
walk_goal = WalkDemoGoal(
Header(),
WalkDemoGoal.WALK,
steps,
AtlasBehaviorStepParams(),
AtlasBehaviorStandParams(),
AtlasBehaviorManipulateParams(),
k_effort,
)
self.client.send_goal(walk_goal)
for step in steps:
# self.loginfo("step: " + str(step))
self.loginfo(
"foot: "
+ str(step.foot_index)
+ " ["
+ str(step.pose.position.x)
+ ", "
+ str(step.pose.position.y)
+ ", "
+ str(theta)
+ "]"
)
self.loginfo("")
self.client.wait_for_result(rospy.Duration(2 * self.params["Stride Duration"]["value"] * len(steps) + 5))
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 build_steps(self, forward, lateral, turn):
L = self.params["Forward Stride Length"]["value"]
L_lat = self.params["Lateral Stride Length"]["value"]
R = self.params["Turn Radius"]["value"]
W = self.params["Stride Width"]["value"]
X = 0
Y = 0
theta = 0
dTheta = 0
if forward != 0:
dTheta = turn * 2 * math.asin(L / (2 * (R + \
self.params["Stride Width"]["value"]/2)))
else:
dTheta = turn * self.params["In Place Turn Size"]["value"]
steps = []
if forward != 0:
dTheta = turn * 2 * math.asin(L / (2 * (R + \
self.params["Stride Width"]["value"]/2)))
else:
dTheta = turn * self.params["In Place Turn Size"]["value"]
steps = []
# This home step doesn't currently do anything, but it's a
# response to bdi not visiting the first step in a trajectory
# home_step = AtlasBehaviorStepData()
# If moving right, first dummy step is on the left
# home_step.foot_index = 1*(lateral < 0)
# home_step.pose.position.y = 0.1
# steps.append(home_step)
prevX = 0
prevY = 0
# Builds the sequence of steps needed
for i in range(self.params["Walk Sequence Length"]["value"]):
# is_right_foot = 1, when stepping with right
is_even = i%2
is_odd = 1 - is_even
is_right_foot = is_even
is_left_foot = is_odd
# left = 1, right = -1
foot = 1 - 2 * is_right_foot
if self.is_static:
theta = (turn != 0) * dTheta
if turn == 0:
X = (forward != 0) * (forward * L)
Y = (lateral != 0) * (is_odd * lateral * L_lat) + \
foot * W / 2
elif forward != 0:
# Radius from point to foot (if turning)
R_foot = R + foot * W/2
# turn > 0 for CCW, turn < 0 for CW
X = forward * turn * R_foot * math.sin(theta)
Y = forward * turn * (R - R_foot*math.cos(theta))
self.debuginfo("R: " + str(R) + " R_foot:" + \
str(R_foot) + " theta: " + str(theta) + \
" math.sin(theta): " + str(math.sin(theta)) + \
" math.cos(theta) + " + str(math.cos(theta)))
elif turn != 0:
X = turn * W/2 * math.sin(theta)
Y = turn * W/2 * math.cos(theta)
else:
theta += (turn != 0) * dTheta
if turn == 0:
X = (forward != 0) * (X + forward * L)
Y = (lateral != 0) * (Y + is_odd * lateral * L_lat) + \
foot * W / 2
elif forward != 0:
# Radius from point to foot (if turning)
R_foot = R + foot * W/2
# turn > 0 for CCW, turn < 0 for CW
X = forward * turn * R_foot * math.sin(theta)
Y = forward * turn * (R - R_foot*math.cos(theta))
self.debuginfo("R: " + str(R) + " R_foot:" + \
str(R_foot) + " theta: " + str(theta) + \
" math.sin(theta): " + str(math.sin(theta)) + \
" math.cos(theta) + " + str(math.cos(theta)))
elif turn != 0:
X = turn * W/2 * math.sin(theta)
Y = turn * W/2 * math.cos(theta)
Q = quaternion_from_euler(0, 0, theta)
step = AtlasBehaviorStepData()
# One step already exists, so add one to index
step.step_index = i
# Alternate between feet, start with left
step.foot_index = is_right_foot
#If moving laterally to the left, start with the right foot
if (lateral > 0):
step.foot_index = is_left_foot
step.duration = self.params["Stride Duration"]["value"]
step.pose.position.x = X
step.pose.position.y = Y
step.pose.position.z = self.params["Step Height"]["value"]
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = self.params["Swing Height"]["value"]
steps.append(step)
# Add final step to bring feet together
is_right_foot = 1 - steps[-1].foot_index
is_even = is_right_foot
# foot = 1 for left, foot = -1 for right
foot = 1 - 2 * is_right_foot
if turn == 0:
Y = Y + foot * W
elif forward != 0:
self.debuginfo("R: " + str(R) + " R_foot:" + \
str(R_foot) + " theta: " + str(theta) + \
" math.sin(theta): " + str(math.sin(theta)) + \
" math.cos(theta) + " + str(math.cos(theta)))
# R_foot is radius to foot
R_foot = R + foot * W/2
#turn > 0 for counter clockwise
X = forward * turn * R_foot * math.sin(theta)
Y = forward * turn * (R - R_foot*math.cos(theta))
else:
X = turn * W/2 * math.sin(theta)
Y = turn * W/2 * math.cos(theta)
Q = quaternion_from_euler(0, 0, theta)
step = AtlasBehaviorStepData()
step.step_index = len(steps)
step.foot_index = is_right_foot
step.duration = self.params["Stride Duration"]["value"]
step.pose.position.x = X
step.pose.position.y = Y
step.pose.position.z = self.params["Step Height"]["value"]
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = self.params["Swing Height"]["value"]
steps.append(step)
return steps
def twist(self, forward, lateral, turn):
steps = []
L = rospy.get_param("Forward_Stride_Length")
L_lat = rospy.get_param("Lateral_Stride_Length")
R = rospy.get_param("Turn_Radius")
W = rospy.get_param("Stride_Width")
X = 0
Y = 0
theta = 0
dTheta = 0
if forward != 0:
dTheta = turn * 2 * math.asin(L / (2 * (R + rospy.get_param("Stride_Width")/2)))
else:
dTheta = turn * rospy.get_param("In_Place_Turn_Size")
steps = []
# This home step doesn't currently do anything, but it's a
# response to bdi not visiting the first step in a trajectory
home_step = AtlasBehaviorStepData()
# If moving right, first dummy step is on the left
home_step.foot_index = 1*(lateral < 0)
home_step.pose.position.y = 0.1
steps.append(home_step)
prevX = 0
prevY = 0
# Builds the sequence of steps needed
for i in range(rospy.get_param("Walk_Sequence_Length")):
theta += (turn != 0) * dTheta
# is_right_foot = 1, when stepping with right
is_even = i%2
is_odd = 1 - is_even
is_right_foot = is_even
is_left_foot = is_odd
# left = 1, right = -1
foot = 1 - 2 * is_right_foot
if turn == 0:
X = (forward != 0) * (X + forward * L)
Y = (lateral != 0) * (Y + is_odd * lateral * L_lat) + foot * W / 2
elif forward != 0:
# Radius from point to foot (if turning)
R_foot = R + foot * W/2
# turn > 0 for CCW, turn < 0 for CW
X = forward * turn * R_foot * math.sin(theta)
Y = forward * turn * (R - R_foot*math.cos(theta))
elif turn != 0:
X = turn * W/2 * math.sin(theta)
Y = turn * W/2 * math.cos(theta)
Q = quaternion_from_euler(0, 0, theta)
step = AtlasBehaviorStepData()
# One step already exists, so add one to index
step.step_index = i+1
# Alternate between feet, start with left
step.foot_index = is_right_foot
#If moving laterally to the left, start with the right foot
if (lateral > 0):
step.foot_index = is_left_foot
step.duration = rospy.get_param("Stride_Duration")
step.pose.position.x = X
step.pose.position.y = Y
step.pose.position.z = rospy.get_param("Step_Height")
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = rospy.get_param("Swing_Height")
steps.append(step)
# Add final step to bring feet together
is_right_foot = 1 - steps[-1].foot_index
is_even = is_right_foot
# foot = 1 for left, foot = -1 for right
foot = 1 - 2 * is_right_foot
if turn == 0:
Y = Y - foot * W
elif forward != 0:
# R_foot is radius to foot
R_foot = R + foot * W/2
#turn > 0 for counter clockwise
X = forward * turn * R_foot * math.sin(theta)
Y = forward * turn * (R - R_foot*math.cos(theta))
else:
X = turn * W/2 * math.sin(theta)
Y = turn * W/2 * math.cos(theta)
Q = quaternion_from_euler(0, 0, theta)
step = AtlasBehaviorStepData()
step.step_index = len(steps)
step.foot_index = is_right_foot
step.duration = rospy.get_param("Stride_Duration")
step.pose.position.x = X
step.pose.position.y = Y
step.pose.position.z = rospy.get_param("Step_Height")
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = rospy.get_param("Swing_Height")
steps.append(step)
# 0 for full BDI control, 255 for PID control
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]
walk_goal = WalkDemoGoal(Header(), WalkDemoGoal.WALK, steps, \
AtlasBehaviorStepParams(), AtlasBehaviorStandParams(), \
AtlasBehaviorManipulateParams(), k_effort )
self.client.send_goal(walk_goal)
self.client.wait_for_result(\
rospy.Duration(2*rospy.get_param("Stride_Duration") * \
len(steps) + 5))
def build_steps(self, forward, lateral, turn):
L = self.params["Forward Stride Length"]["value"]
L_lat = self.params["Lateral Stride Length"]["value"]
R = self.params["Turn Radius"]["value"]
W = self.params["Stride Width"]["value"]
X = 0
Y = 0
theta = 0
dTheta = 0
if forward != 0:
dTheta = turn * 2 * math.asin(L / (2 * (R + \
self.params["Stride Width"]["value"]/2)))
else:
dTheta = turn * self.params["In Place Turn Size"]["value"]
steps = []
if forward != 0:
dTheta = turn * 2 * math.asin(L / (2 * (R + \
self.params["Stride Width"]["value"]/2)))
else:
dTheta = turn * self.params["In Place Turn Size"]["value"]
steps = []
# This home step doesn't currently do anything, but it's a
# response to bdi not visiting the first step in a trajectory
# home_step = AtlasBehaviorStepData()
# If moving right, first dummy step is on the left
# home_step.foot_index = 1*(lateral < 0)
# home_step.pose.position.y = 0.1
# steps.append(home_step)
prevX = 0
prevY = 0
# Builds the sequence of steps needed
for i in range(self.params["Walk Sequence Length"]["value"]):
# is_right_foot = 1, when stepping with right
is_even = i % 2
is_odd = 1 - is_even
is_right_foot = is_even
is_left_foot = is_odd
# left = 1, right = -1
foot = 1 - 2 * is_right_foot
if self.is_static:
theta = (turn != 0) * dTheta
if turn == 0:
X = (forward != 0) * (forward * L)
Y = (lateral != 0) * (is_odd * lateral * L_lat) + \
foot * W / 2
elif forward != 0:
# Radius from point to foot (if turning)
R_foot = R + foot * W / 2
# turn > 0 for CCW, turn < 0 for CW
X = forward * turn * R_foot * math.sin(theta)
Y = forward * turn * (R - R_foot * math.cos(theta))
self.debuginfo("R: " + str(R) + " R_foot:" + \
str(R_foot) + " theta: " + str(theta) + \
" math.sin(theta): " + str(math.sin(theta)) + \
" math.cos(theta) + " + str(math.cos(theta)))
elif turn != 0:
X = turn * W / 2 * math.sin(theta)
Y = turn * W / 2 * math.cos(theta)
else:
theta += (turn != 0) * dTheta
if turn == 0:
X = (forward != 0) * (X + forward * L)
Y = (lateral != 0) * (Y + is_odd * lateral * L_lat) + \
foot * W / 2
elif forward != 0:
# Radius from point to foot (if turning)
R_foot = R + foot * W / 2
# turn > 0 for CCW, turn < 0 for CW
X = forward * turn * R_foot * math.sin(theta)
Y = forward * turn * (R - R_foot * math.cos(theta))
self.debuginfo("R: " + str(R) + " R_foot:" + \
str(R_foot) + " theta: " + str(theta) + \
" math.sin(theta): " + str(math.sin(theta)) + \
" math.cos(theta) + " + str(math.cos(theta)))
elif turn != 0:
X = turn * W / 2 * math.sin(theta)
Y = turn * W / 2 * math.cos(theta)
Q = quaternion_from_euler(0, 0, theta)
step = AtlasBehaviorStepData()
# One step already exists, so add one to index
step.step_index = i
# Alternate between feet, start with left
step.foot_index = is_right_foot
#If moving laterally to the left, start with the right foot
if (lateral > 0):
step.foot_index = is_left_foot
step.duration = self.params["Stride Duration"]["value"]
step.pose.position.x = X
step.pose.position.y = Y
step.pose.position.z = self.params["Step Height"]["value"]
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = self.params["Swing Height"]["value"]
steps.append(step)
# Add final step to bring feet together
is_right_foot = 1 - steps[-1].foot_index
is_even = is_right_foot
# foot = 1 for left, foot = -1 for right
foot = 1 - 2 * is_right_foot
if turn == 0:
Y = Y + foot * W
elif forward != 0:
self.debuginfo("R: " + str(R) + " R_foot:" + \
str(R_foot) + " theta: " + str(theta) + \
" math.sin(theta): " + str(math.sin(theta)) + \
" math.cos(theta) + " + str(math.cos(theta)))
# R_foot is radius to foot
R_foot = R + foot * W / 2
#turn > 0 for counter clockwise
X = forward * turn * R_foot * math.sin(theta)
Y = forward * turn * (R - R_foot * math.cos(theta))
else:
X = turn * W / 2 * math.sin(theta)
Y = turn * W / 2 * math.cos(theta)
Q = quaternion_from_euler(0, 0, theta)
step = AtlasBehaviorStepData()
step.step_index = len(steps)
step.foot_index = is_right_foot
step.duration = self.params["Stride Duration"]["value"]
step.pose.position.x = X
step.pose.position.y = Y
step.pose.position.z = self.params["Step Height"]["value"]
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = self.params["Swing Height"]["value"]
steps.append(step)
return steps
def _build_steps():
L = 0.15
L_lat = 0.15
R = 2
W = 0.2
X = 0
Y = 0
theta = 0
dTheta = 0
steps = []
# Builds the sequence of steps needed
for i in range(50):
is_even = i%2
is_odd = 1 - is_even
is_right_foot = is_even
is_left_foot = is_odd
# left = 1, right = -1
foot = 1 - 2 * is_right_foot
X = (1 != 0) * (X + 1 * L)
Y = (0 != 0) * (Y + is_odd * 0 * L_lat) + foot * W / 2
Q = quaternion_from_euler(0, 0, theta)
step = AtlasBehaviorStepData()
# One step already exists, so add one to index
step.step_index = i
# Alternate between feet, start with left
step.foot_index = is_right_foot
step.duration = 0.63
step.pose.position.x = X
step.pose.position.y = Y
step.pose.position.z = 0.0
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = 0.1
steps.append(step)
# Add final step to bring feet together
is_right_foot = 1 - steps[-1].foot_index
is_even = is_right_foot
# foot = 1 for left, foot = -1 for right
foot = 1 - 2 * is_right_foot
Y = Y + foot * W
Q = quaternion_from_euler(0, 0, theta)
step = AtlasBehaviorStepData()
step.step_index = len(steps)
step.foot_index = is_right_foot
step.duration = 0.63
step.pose.position.x = X
step.pose.position.y = Y
step.pose.position.z = 0.0
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = 0.1
steps.append(step)
return steps
def bridge_qual_1(self):
s = []
# Step to first block
s.append([0.5, 0, 0])
# Inch forward on first block
for i in range(3):
s.append([0.03, 0, 0])
# Turn toward second block
for i in range(4):
s.append([0, 0, 0.196])
# Step onto second block
s.append([0.4, 0, 0])
s.append([0.4, 0, 0])
# Turn toward third block
for i in range(4):
s.append([0, 0, -0.196])
# Inch forward on second block
for i in range(3):
s.append([0.03, 0, 0])
# Step to third block
s.append([0.4, 0, 0])
#Inch forward on third block
for i in range(4):
s.append([0.03, -0.01, 0])
# Turn toward fourth block
for i in range(4):
s.append([0, 0, -0.196])
#Step onto fourth block
s.append([0.4, 0, 0])
s.append([0.4, 0, 0])
# Turn toward goal
for i in range(4):
s.append([0, 0, 0.196])
#Step onto platform
s.append([0.4, 0, 0])
s.append([0.4, 0, 0])
#Walk through the goal
for i in range(10):
s.append([0.3, 0, 0])
steps = []
home_step = AtlasBehaviorStepData()
# If moving right, first dummy step is on the left
home_step.foot_index = 1
home_step.pose.position.y = -0.1
steps.append(home_step)
X = 0
Y = 0
W = 0.2
theta = 0
# Build trajectory steps
for i in range(len(s)):
is_even = i % 2
is_odd = 1 - is_even
is_right_foot = is_even
foot = 1 - 2 * is_even
turn = 0
traj = s[i]
theta += traj[2]
if traj[2] > 0:
turn = 1
elif traj[2] < 0:
turn = -1
X += traj[0] * math.sin(3.14 / 2 -
theta) + traj[1] * math.sin(theta)
Y += traj[0] * math.cos(3.14 / 2 -
theta) + traj[1] * math.cos(theta)
print("[" + str(X) + ", " + str(Y) + ", " + str(theta))
Q = quaternion_from_euler(0, 0, theta)
step = AtlasBehaviorStepData()
# One step already exists, so add one to index
step.step_index = i + 1
# Alternate between feet, start with left
step.foot_index = is_right_foot
step.duration = 0.63
step.pose.position.x = X - foot * W / 2 * math.sin(theta)
step.pose.position.y = Y + foot * W / 2 * math.cos(theta)
step.pose.position.z = 0
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = 0.3
steps.append(step)
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]
walk_goal = WalkDemoGoal(Header(), WalkDemoGoal.WALK, steps, \
AtlasBehaviorStepParams(), AtlasBehaviorStandParams(), \
AtlasBehaviorManipulateParams(), k_effort )
self.client.send_goal(walk_goal)
for step in steps:
print("foot: " + str(step.foot_index) + \
" [" + str(step.pose.position.x) + \
", " + str(step.pose.position.y) + ", " + str(theta) + "]")
def bridge_qual_1(self):
s = []
# Step to first block
s.append([0.5, 0, 0])
# Inch forward on first block
for i in range(3):
s.append([0.03, 0, 0])
# Turn toward second block
for i in range(4):
s.append([0, 0, 0.196])
# Step onto second block
s.append([0.4, 0, 0])
s.append([0.4, 0, 0])
# Turn toward third block
for i in range(4):
s.append([0, 0, -0.196])
# Inch forward on second block
for i in range(3):
s.append([0.03, 0, 0])
# Step to third block
s.append([0.4, 0, 0])
#Inch forward on third block
for i in range(4):
s.append([0.03, -0.01, 0])
# Turn toward fourth block
for i in range(4):
s.append([0, 0, -0.196])
#Step onto fourth block
s.append([0.4, 0, 0])
s.append([0.4, 0, 0])
# Turn toward goal
for i in range(4):
s.append([0, 0, 0.196])
#Step onto platform
s.append([0.4, 0, 0])
s.append([0.4, 0, 0])
#Walk through the goal
for i in range(10):
s.append([0.3, 0, 0])
steps = []
home_step = AtlasBehaviorStepData()
# If moving right, first dummy step is on the left
home_step.foot_index = 1
home_step.pose.position.y = -0.1
steps.append(home_step)
X = 0
Y = 0
W = 0.2
theta = 0
# Build trajectory steps
for i in range(len(s)):
is_even = i%2
is_odd = 1-is_even
is_right_foot = is_even
foot = 1 - 2*is_even
turn = 0
traj = s[i]
theta += traj[2]
if traj[2] > 0:
turn = 1
elif traj[2] < 0:
turn = -1
X += traj[0]*math.sin(3.14/2 - theta) + traj[1]*math.sin(theta)
Y += traj[0]*math.cos(3.14/2 - theta) + traj[1]*math.cos(theta)
print("[" + str(X) + ", " + str(Y) + ", " + str(theta))
Q = quaternion_from_euler(0, 0, theta)
step = AtlasBehaviorStepData()
# One step already exists, so add one to index
step.step_index = i+1
# Alternate between feet, start with left
step.foot_index = is_right_foot
step.duration = 0.63
step.pose.position.x = X - foot * W/2 * math.sin(theta)
step.pose.position.y = Y + foot * W/2 * math.cos(theta)
step.pose.position.z = 0
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = 0.3
steps.append(step)
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]
walk_goal = WalkDemoGoal(Header(), WalkDemoGoal.WALK, steps, \
AtlasBehaviorStepParams(), AtlasBehaviorStandParams(), \
AtlasBehaviorManipulateParams(), k_effort )
self.client.send_goal(walk_goal)
for step in steps:
print("foot: " + str(step.foot_index) + \
" [" + str(step.pose.position.x) + \
", " + str(step.pose.position.y) + ", " + str(theta) + "]")
