(self)
return action, contact_forces
Computes the torque for stance legs.
Computes the torque for stance legs.
Computes the torque for stance legs .
def get_action(self):
    """Computes the torque for stance legs."""
    desired_com_position = np.array((0., 0., self._desired_body_height),
                                    dtype=np.float64)
    desired_com_velocity = np.array(
        (self.desired_speed[0], self.desired_speed[1], 0.), dtype=np.float64)
    desired_com_roll_pitch_yaw = np.array((0., 0., 0.), dtype=np.float64)
    desired_com_angular_velocity = np.array(
        (0., 0., self.desired_twisting_speed), dtype=np.float64)
    foot_contact_state = np.array(
        [(leg_state in (gait_generator_lib.LegState.STANCE,
                        gait_generator_lib.LegState.EARLY_CONTACT))
         for leg_state in self._gait_generator.desired_leg_state],
        dtype=np.int32)

    # We use the body yaw aligned world frame for MPC computation.
    com_roll_pitch_yaw = np.array(self._robot.GetBaseRollPitchYaw(),
                                  dtype=np.float64)
    com_roll_pitch_yaw[2] = 0

    #predicted_contact_forces=[0]*self._num_legs*_FORCE_DIMENSION
    # print("Com Vel: {}".format(self._state_estimator.com_velocity_body_frame))
    # print("Com RPY: {}".format(self._robot.GetBaseRollPitchYawRate()))
    # print("Com RPY Rate: {}".format(self._robot.GetBaseRollPitchYawRate()))
    p.submitProfileTiming("predicted_contact_forces")
    predicted_contact_forces = self._cpp_mpc.compute_contact_forces(
        [0],  #com_position
        np.asarray(self._state_estimator.com_velocity_body_frame,
                   dtype=np.float64),  #com_velocity
        np.array(com_roll_pitch_yaw, dtype=np.float64),  #com_roll_pitch_yaw
        # Angular velocity in the yaw aligned world frame is actually different
        # from rpy rate. We use it here as a simple approximation.
        np.asarray(self._robot.GetBaseRollPitchYawRate(),
                   dtype=np.float64),  #com_angular_velocity
        foot_contact_state,  #foot_contact_states
        np.array(self._robot.GetFootPositionsInBaseFrame().flatten(),
                 dtype=np.float64),  #foot_positions_base_frame
        self._friction_coeffs,  #foot_friction_coeffs
        desired_com_position,  #desired_com_position
        desired_com_velocity,  #desired_com_velocity
        desired_com_roll_pitch_yaw,  #desired_com_roll_pitch_yaw
        desired_com_angular_velocity  #desired_com_angular_velocity
    )
    p.submitProfileTiming()
    # sol = np.array(predicted_contact_forces).reshape((-1, 12))
    # x_dim = np.array([0, 3, 6, 9])
    # y_dim = x_dim + 1
    # z_dim = y_dim + 1
    # print("Y_forces: {}".format(sol[:, y_dim]))

    contact_forces = {}
    for i in range(self._num_legs):
      contact_forces[i] = np.array(
          predicted_contact_forces[i * _FORCE_DIMENSION:(i + 1) *
                                   _FORCE_DIMENSION])
    action = {}
    for leg_id, force in contact_forces.items():
      # While "Lose Contact" is useful in simulation, in real environment it's
      # susceptible to sensor noise. Disabling for now.
      # if self._gait_generator.leg_state[
      #     leg_id] == gait_generator_lib.LegState.LOSE_CONTACT:
      #   force = (0, 0, 0)
      motor_torques = self._robot.MapContactForceToJointTorques(leg_id, force)
      for joint_id, torque in motor_torques.items():
        action[joint_id] = (0, 0, 0, 0, torque)

    return action, contact_forces
def get_action ( self ) : desired_com_position = np . array ( ( 0. , 0. , self . _desired_body_height ) , dtype = np . float64 ) desired_com_velocity = np . array ( ( self . desired_speed [ 0 ] , self . desired_speed [ 1 ] , 0. ) , dtype = np . float64 ) desired_com_roll_pitch_yaw = np . array ( ( 0. , 0. , 0. ) , dtype = np . float64 ) desired_com_angular_velocity = np . array ( ( 0. , 0. , self . desired_twisting_speed ) , dtype = np . float64 ) foot_contact_state = np . array ( [ ( leg_state in ( gait_generator_lib . LegState . STANCE , gait_generator_lib . LegState . EARLY_CONTACT ) ) for leg_state in self . _gait_generator . desired_leg_state ] , dtype = np . int32 ) # We use the body yaw aligned world frame for MPC computation. com_roll_pitch_yaw = np . array ( self . _robot . GetBaseRollPitchYaw ( ) , dtype = np . float64 ) com_roll_pitch_yaw [ 2 ] = 0 #predicted_contact_forces=[0]*self._num_legs*_FORCE_DIMENSION # print("Com Vel: {}".format(self._state_estimator.com_velocity_body_frame)) # print("Com RPY: {}".format(self._robot.GetBaseRollPitchYawRate())) # print("Com RPY Rate: {}".format(self._robot.GetBaseRollPitchYawRate())) p . submitProfileTiming ( "predicted_contact_forces" ) predicted_contact_forces = self . _cpp_mpc . compute_contact_forces ( [ 0 ] , #com_position np . asarray ( self . _state_estimator . com_velocity_body_frame , dtype = np . float64 ) , #com_velocity np . array ( com_roll_pitch_yaw , dtype = np . float64 ) , #com_roll_pitch_yaw # Angular velocity in the yaw aligned world frame is actually different # from rpy rate. We use it here as a simple approximation. np . asarray ( self . _robot . GetBaseRollPitchYawRate ( ) , dtype = np . float64 ) , #com_angular_velocity foot_contact_state , #foot_contact_states np . array ( self . _robot . GetFootPositionsInBaseFrame ( ) . flatten ( ) , dtype = np . float64 ) , #foot_positions_base_frame self . _friction_coeffs , #foot_friction_coeffs desired_com_position , #desired_com_position desired_com_velocity , #desired_com_velocity desired_com_roll_pitch_yaw , #desired_com_roll_pitch_yaw desired_com_angular_velocity #desired_com_angular_velocity ) p . submitProfileTiming ( ) # sol = np.array(predicted_contact_forces).reshape((-1, 12)) # x_dim = np.array([0, 3, 6, 9]) # y_dim = x_dim + 1 # z_dim = y_dim + 1 # print("Y_forces: {}".format(sol[:, y_dim])) contact_forces = { } for i in range ( self . _num_legs ) : contact_forces [ i ] = np . array ( predicted_contact_forces [ i * _FORCE_DIMENSION : ( i + 1 ) * _FORCE_DIMENSION ] ) action = { } for leg_id , force in contact_forces . items ( ) : # While "Lose Contact" is useful in simulation, in real environment it's # susceptible to sensor noise. Disabling for now. # if self._gait_generator.leg_state[ #     leg_id] == gait_generator_lib.LegState.LOSE_CONTACT: #   force = (0, 0, 0) motor_torques = self . _robot . MapContactForceToJointTorques ( leg_id , force ) for joint_id , torque in motor_torques . items ( ) : action [ joint_id ] = ( 0 , 0 , 0 , 0 , torque ) return action , contact_forces