#include "PinDragIK.h"
#include "Body.h"
#include "JointPath.h"
#include <cnoid/EigenUtil>
#include <map>
#include <iostream>
#include <algorithm>