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) + "]")