setupKinematics method

Future<void> setupKinematics()

Implementation

Future<void> setupKinematics() async{
  final kinematicsModelId = library['kinematicsModels']?.keys;
  final kinematicsSceneId = library['kinematicsScenes']?.keys;
  final visualSceneId = library['visualScenes']?.keys;
  if ( kinematicsModelId == null || kinematicsModelId.isEmpty || kinematicsSceneId == null || kinematicsSceneId.isEmpty) return;

  final kinematicsModel = await getKinematicsModel( kinematicsModelId.toList()[0] );
  final kinematicsScene = getKinematicsScene( kinematicsSceneId.toList()[0] );
  final visualScene = await getVisualScene( visualSceneId!.toList()[0] );

  final bindJointAxis = kinematicsScene['bindJointAxis'];
  final jointMap = {};

  connect( jointIndex, XmlElement visualElement ) {
    final visualElementName = visualElement.getAttribute( 'name' );
    final joint = kinematicsModel['joints'][ jointIndex ];

    visualScene.traverse( ( object ) {
      if ( object.name == visualElementName ) {
        jointMap[ jointIndex ] = {
          'object': object,
          'transforms': buildTransformList( visualElement ),
          'joint': joint,
          'position': joint['zeroPosition']
        };
      }
    } );
  }

  for (int i = 0, l = bindJointAxis.length; i < l; i ++ ) {
    final axis = bindJointAxis[ i ];
    List<XmlNode> targetElement = collada.xpath( '//translate[@sid="${axis['target']}"]' ).toList();
    targetElement += collada.xpath( '//rotate[@sid="${axis['target']}"]' ).toList();
    targetElement += collada.xpath( '//matrix[@sid="${axis['target']}"]' ).toList();
    targetElement += collada.xpath( '//scale[@sid="${axis['target']}"]' ).toList();
    if ( targetElement.isNotEmpty) {
      final parentVisualElement = targetElement[0].parentElement;
      connect( axis['jointIndex'], parentVisualElement! );
    }
  }

  final m0 = Matrix4.identity();
  kinematics = KinematicsData(
    joints: kinematicsModel['joints'],
    getJointValue: ( jointIndex ) {
      final jointData = jointMap[ jointIndex ];

      if ( jointData != null) {
        return jointData['position'];
      }
      console.warning( 'ColladaLoader: Joint $jointIndex doesn\'t exist.' );
      return null;
    },
    setJointValue: ( jointIndex, value ) {
      final jointData = jointMap[ jointIndex ];
      if ( jointData != null) {
        final joint = jointData['joint'];

        if ( value > joint['limits']['max'] || value < joint['limits']['min'] ) {
          console.warning( 'ColladaLoader: Joint $jointIndex value $value outside of limits (min: ${joint['limits']['min']}, max: ${joint['limits']['max']}).' );
        }
        else if ( joint['static'] == true) {
          console.warning( 'ColladaLoader: Joint $jointIndex is static.' );
        }
        else {
          final Group object = jointData['object'];
          final Vector3 axis = joint['axis'];
          final List transforms = jointData['transforms'];

          matrix.identity();

          // each update, we have to apply all transforms in the correct order

          for (int i = 0; i < transforms.length; i ++ ) {
            final Map<String,dynamic> transform = transforms[ i ];

            // if there is a connection of the transform node with a joint, apply the joint value
            if ( transform['sid'] != null && transform['sid'].contains( jointIndex )) {
              switch ( joint['type'] ) {
                case 'revolute':
                  matrix.multiply( m0.makeRotationAxis( axis, MathUtils.degToRad( value ) ) );
                  break;
                case 'prismatic':
                  matrix.multiply( m0.makeTranslation( axis.x * value, axis.y * value, axis.z * value ) );
                  break;
                default:
                  console.warning( 'ColladaLoader: Unknown joint type: ${joint['type']}');
                  break;
              }
            }
            else {
              switch ( transform['type'] ) {
                case 'matrix':
                  matrix.multiply( transform['obj'] );
                  break;
                case 'translate':
                  matrix.multiply( m0.makeTranslation( transform['obj'].x, transform['obj'].y, transform['obj'].z ) );
                  break;
                case 'scale':
                  matrix.scale( transform['obj'] );
                  break;
                case 'rotate':
                  matrix.multiply( m0.makeRotationAxis( transform['obj'], transform['angle'] ) );
                  break;
              }
            }
          }

          object.matrix.setFrom( matrix );
          object.matrix.decompose( object.position, object.quaternion, object.scale );

          jointMap[ jointIndex ]['position'] = value;
        }
      }
      else {
        console.info( 'ColladaLoader: $jointIndex does not exist.' );
      }
    }
  );
}