<commit_before>#ifndef _MYWINDOW_
#define _MYWINDOW_

#include "yui/Win3D.h"
#include "integration/EulerIntegrator.h"
#include "integration/RK4Integrator.h"
#include "collision/CollisionShapes.h"
#include "collision/CollisionSkeleton.h"
#include "utils/Parser/dart_parser/DartLoader.h"
#include "robotics/World.h"
#include "utils/Paths.h"
#include "robotics/World.h"
#include "dynamics/SkeletonDynamics.h"
#include "robotics/Object.h"
#include "kinematics/ShapeCube.h"
#include "dynamics/BodyNodeDynamics.h"
#include "kinematics/Joint.h"

using namespace collision_checking;
using namespace std;

namespace dynamics{
    class SkeletonDynamics;
    class ContactDynamics;
}

namespace integration{
    class IntegrableSystem;
}

class MyWindow : public yui::Win3D, public integration::IntegrableSystem {
public:
MyWindow(): Win3D() {
        DartLoader dl;
        mWorld = dl.parseWorld(DART_DATA_PATH"/scenes/DesktopArm.urdf");
        
        // Add ground plane
        robotics::Object* ground = new robotics::Object();
        ground->addDefaultRootNode();
        dynamics::BodyNodeDynamics* node = new dynamics::BodyNodeDynamics();
        node->setShape(new kinematics::ShapeCube(Eigen::Vector3d(10.0, 10.0, 0.0001), 1.0));
        kinematics::Joint* joint = new kinematics::Joint(ground->getRoot(), node);
        ground->addNode(node);
		ground->setPositionZ(-0.1);
        
		ground->initSkel();
		ground->update();
		ground->setImmobileState(true);
        mWorld->addObject(ground);
		mWorld->rebuildCollision();

        mBackground[0] = 1.0;
        mBackground[1] = 1.0;
        mBackground[2] = 1.0;
        mBackground[3] = 1.0;

        mPlayState = PAUSED;
        mSimFrame = 0;
        mPlayFrame = 0;
        mMovieFrame = 0;


        mShowMarker = false;
        mForce = Eigen::Vector3d::Zero();

        mPersp = 45.f;
        mTrans[1] = 300.f;
    
        mGravity = Eigen::Vector3d(0.0, 0.0, -9.8);
        mTimeStep = 1.0/1000.0;
        
        int sumNDofs = 0;
        mIndices.push_back(sumNDofs);
        for (unsigned int i = 0; i < mWorld->getNumSkeletons(); i++) {
            int nDofs = mWorld->getSkeleton(i)->getNumDofs();
            sumNDofs += nDofs;
            mIndices.push_back(sumNDofs);
        }

        initDyn();

        std::cout << 
            "\nKeybindings:\n" <<
            "\n" <<
            "s: start or continue simulating.\n" <<
            "\n" <<
            "p: start or continue playback.\n" <<
            "r, t: move to start or end of playback.\n" <<
            "[, ]: step through playback by one frame.\n" <<
            "\n" <<
            "m: start or continue movie recording.\n" <<
            "\n" <<
            "space: pause/unpause whatever is happening.\n" <<
            "\n" <<
            "q, escape: quit.\n" <<
            std::endl;
    }

    virtual void draw();
    virtual void keyboard(unsigned char key, int x, int y);<commit_msg>Planning example not crashing anymore.<commit_after>#ifndef _MYWINDOW_
#define _MYWINDOW_

#include "yui/Win3D.h"
#include "integration/EulerIntegrator.h"
#include "integration/RK4Integrator.h"
#include "collision/CollisionShapes.h"
#include "collision/CollisionSkeleton.h"
#include "utils/Parser/dart_parser/DartLoader.h"
#include "robotics/World.h"
#include "utils/Paths.h"
#include "robotics/World.h"
#include "dynamics/SkeletonDynamics.h"
#include "robotics/Object.h"
#include "kinematics/ShapeCube.h"
#include "dynamics/BodyNodeDynamics.h"
#include "kinematics/Joint.h"
#include "kinematics/ShapeMesh.h"

using namespace collision_checking;
using namespace std;

namespace dynamics{
    class SkeletonDynamics;
    class ContactDynamics;
}

namespace integration{
    class IntegrableSystem;
}

class MyWindow : public yui::Win3D, public integration::IntegrableSystem {
public:
MyWindow(): Win3D() {
        DartLoader dl;
        mWorld = dl.parseWorld(DART_DATA_PATH"/scenes/DesktopArm.urdf");
        
        // Add ground plane
        robotics::Object* ground = new robotics::Object();
        ground->addDefaultRootNode();
        dynamics::BodyNodeDynamics* node = new dynamics::BodyNodeDynamics();
        const aiScene* m3d = kinematics::ShapeMesh::loadMesh( DART_DATA_PATH"/scenes/objects/ground.blend");
        kinematics::ShapeMesh* shape = new kinematics::ShapeMesh(Eigen::Vector3d(1.0, 1.0, 1.0), 0.0, m3d);
        shape->setCollisionMesh(m3d);
        shape->setVizMesh(m3d);
        shape->setMass(1.0);
        shape->setInertia(Eigen::Matrix3d::Identity());
        node->setShape(shape);
        //node->setShape(new kinematics::ShapeCube(Eigen::Vector3d(10.0, 10.0, 0.0001), 1.0));
        kinematics::Joint* joint = new kinematics::Joint(ground->getRoot(), node);
        ground->addNode(node);
		ground->setPositionZ(-0.1);
        
		ground->initSkel();
		ground->update();
		ground->setImmobileState(true);
        mWorld->addObject(ground);
		mWorld->rebuildCollision();

        mBackground[0] = 1.0;
        mBackground[1] = 1.0;
        mBackground[2] = 1.0;
        mBackground[3] = 1.0;

        mPlayState = PAUSED;
        mSimFrame = 0;
        mPlayFrame = 0;
        mMovieFrame = 0;


        mShowMarker = false;
        mForce = Eigen::Vector3d::Zero();

        mPersp = 45.f;
        mTrans[1] = 300.f;
    
        mGravity = Eigen::Vector3d(0.0, 0.0, -9.8);
        mTimeStep = 1.0/1000.0;
        
        int sumNDofs = 0;
        mIndices.push_back(sumNDofs);
        for (unsigned int i = 0; i < mWorld->getNumSkeletons(); i++) {
            int nDofs = mWorld->getSkeleton(i)->getNumDofs();
            sumNDofs += nDofs;
            mIndices.push_back(sumNDofs);
        }

        initDyn();

       std::cout << 
            "\nKeybindings:\n" <<
            "\n" <<
            "s: start or continue simulating.\n" <<
            "\n" <<
            "p: start or continue playback.\n" <<
            "r, t: move to start or end of playback.\n" <<
            "[, ]: step through playback by one frame.\n" <<
            "\n" <<
            "m: start or continue movie recording.\n" <<
            "\n" <<
            "space: pause/unpause whatever is happening.\n" <<
            "\n" <<
            "q, escape: quit.\n" <<
            std::endl;
    }

    virtual void draw();
    virtual void keyboard(unsigned char key, int x, int y);
{'fraction_non_alphanumeric': 0.1424735428842205, 'fraction_numerical': 0.020849786763544462, 'mean_word_length': 2.4733955019199123, 'pattern_counts': {'":': 0, '<': 61, '<?xml version=': 0, '>': 34, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5155167', 'n_tokens_mistral': 2202, 'n_tokens_neox': 2066, 'n_words': 419}