(self, robot_state)
Propagate current state estimate with new accelerometer reading.
Propagate current state estimate with new accelerometer reading.
Propagate current state estimate with new accelerometer reading .
def update(self, robot_state):
    """Propagate current state estimate with new accelerometer reading."""
    delta_time_s = self._compute_delta_time(robot_state)
    sensor_acc = np.array(robot_state.imu.accelerometer)
    base_orientation = self.robot.GetBaseOrientation()
    rot_mat = self.robot.pybullet_client.getMatrixFromQuaternion(
        base_orientation)
    rot_mat = np.array(rot_mat).reshape((3, 3))
    calibrated_acc = rot_mat.dot(sensor_acc) + np.array([0., 0., -9.8])
    self.filter.predict(u=calibrated_acc * delta_time_s)

    # Correct estimation using contact legs
    observed_velocities = []
    foot_contact = self.robot.GetFootContacts()
    for leg_id in range(4):
      if foot_contact[leg_id]:
        jacobian = self.robot.ComputeJacobian(leg_id)
        # Only pick the jacobian related to joint motors
        joint_velocities = self.robot.motor_velocities[leg_id *
                                                       3:(leg_id + 1) * 3]
        leg_velocity_in_base_frame = jacobian.dot(joint_velocities)
        base_velocity_in_base_frame = -leg_velocity_in_base_frame[:3]
        observed_velocities.append(rot_mat.dot(base_velocity_in_base_frame))

    if observed_velocities:
      observed_velocities = np.mean(observed_velocities, axis=0)
      self.filter.update(observed_velocities)

    vel_x = self.moving_window_filter_x.calculate_average(self.filter.x[0])
    vel_y = self.moving_window_filter_y.calculate_average(self.filter.x[1])
    vel_z = self.moving_window_filter_z.calculate_average(self.filter.x[2])
    self._estimated_velocity = np.array([vel_x, vel_y, vel_z])
def update ( self , robot_state ) : delta_time_s = self . _compute_delta_time ( robot_state ) sensor_acc = np . array ( robot_state . imu . accelerometer ) base_orientation = self . robot . GetBaseOrientation ( ) rot_mat = self . robot . pybullet_client . getMatrixFromQuaternion ( base_orientation ) rot_mat = np . array ( rot_mat ) . reshape ( ( 3 , 3 ) ) calibrated_acc = rot_mat . dot ( sensor_acc ) + np . array ( [ 0. , 0. , - 9.8 ] ) self . filter . predict ( u = calibrated_acc * delta_time_s ) # Correct estimation using contact legs observed_velocities = [ ] foot_contact = self . robot . GetFootContacts ( ) for leg_id in range ( 4 ) : if foot_contact [ leg_id ] : jacobian = self . robot . ComputeJacobian ( leg_id ) # Only pick the jacobian related to joint motors joint_velocities = self . robot . motor_velocities [ leg_id * 3 : ( leg_id + 1 ) * 3 ] leg_velocity_in_base_frame = jacobian . dot ( joint_velocities ) base_velocity_in_base_frame = - leg_velocity_in_base_frame [ : 3 ] observed_velocities . append ( rot_mat . dot ( base_velocity_in_base_frame ) ) if observed_velocities : observed_velocities = np . mean ( observed_velocities , axis = 0 ) self . filter . update ( observed_velocities ) vel_x = self . moving_window_filter_x . calculate_average ( self . filter . x [ 0 ] ) vel_y = self . moving_window_filter_y . calculate_average ( self . filter . x [ 1 ] ) vel_z = self . moving_window_filter_z . calculate_average ( self . filter . x [ 2 ] ) self . _estimated_velocity = np . array ( [ vel_x , vel_y , vel_z ] )