106 lines
2.9 KiB
C++
106 lines
2.9 KiB
C++
/* SPDX-FileCopyrightText: 2009 Ruben Smits
|
|
*
|
|
* SPDX-License-Identifier: LGPL-2.1-or-later */
|
|
|
|
/** \file
|
|
* \ingroup intern_itasc
|
|
*/
|
|
|
|
#ifndef SCENE_HPP_
|
|
#define SCENE_HPP_
|
|
|
|
#include "eigen_types.hpp"
|
|
|
|
#include "WorldObject.hpp"
|
|
#include "ConstraintSet.hpp"
|
|
#include "Solver.hpp"
|
|
|
|
#include <map>
|
|
|
|
namespace iTaSC {
|
|
|
|
class SceneLock;
|
|
|
|
class Scene {
|
|
friend class SceneLock;
|
|
public:
|
|
enum SceneParam {
|
|
MIN_TIMESTEP = 0,
|
|
MAX_TIMESTEP,
|
|
|
|
COUNT
|
|
};
|
|
|
|
|
|
Scene();
|
|
virtual ~Scene();
|
|
|
|
bool addObject(const std::string& name, Object* object, UncontrolledObject* base=&Object::world, const std::string& baseFrame="");
|
|
bool addConstraintSet(const std::string& name, ConstraintSet* task,const std::string& object1,const std::string& object2,const std::string& ee1="",const std::string& ee2="");
|
|
bool addSolver(Solver* _solver);
|
|
bool addCache(Cache* _cache);
|
|
bool initialize();
|
|
bool update(double timestamp, double timestep, unsigned int numsubstep=1, bool reiterate=false, bool cache=true, bool interpolate=true);
|
|
bool setParam(SceneParam paramId, double value);
|
|
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
|
|
|
private:
|
|
e_matrix m_A,m_B,m_Atemp,m_Wq,m_Jf,m_Jq,m_Ju,m_Cf,m_Cq,m_Jf_inv;
|
|
e_matrix6 m_Vf,m_Uf;
|
|
e_vector m_Wy,m_ydot,m_qdot,m_xdot;
|
|
e_vector6 m_Sf,m_tempf;
|
|
double m_minstep;
|
|
double m_maxstep;
|
|
unsigned int m_ncTotal,m_nqTotal,m_nuTotal,m_nsets;
|
|
std::vector<bool> m_ytask;
|
|
|
|
Solver* m_solver;
|
|
Cache* m_cache;
|
|
|
|
|
|
struct Object_struct{
|
|
Object* object;
|
|
UncontrolledObject* base;
|
|
unsigned int baseFrameIndex;
|
|
Range constraintrange;
|
|
Range jointrange;
|
|
Range coordinaterange; // Xu range of base when object is controlled
|
|
// Xu range of object when object is uncontrolled
|
|
|
|
Object_struct(Object* _object,UncontrolledObject* _base,unsigned int _baseFrameIndex,Range nq_range,Range nc_range,Range nu_range):
|
|
object(_object),base(_base),baseFrameIndex(_baseFrameIndex),constraintrange(nc_range),jointrange(nq_range),coordinaterange(nu_range)
|
|
{};
|
|
};
|
|
typedef std::map<std::string,Object_struct*> ObjectMap;
|
|
|
|
struct ConstraintSet_struct{
|
|
ConstraintSet* task;
|
|
ObjectMap::iterator object1;
|
|
ObjectMap::iterator object2;
|
|
Range constraintrange;
|
|
Range featurerange;
|
|
unsigned int ee1index;
|
|
unsigned int ee2index;
|
|
ConstraintSet_struct(ConstraintSet* _task,
|
|
ObjectMap::iterator _object1,unsigned int _ee1index,
|
|
ObjectMap::iterator _object2,unsigned int _ee2index,
|
|
Range nc_range,Range coord_range):
|
|
task(_task),
|
|
object1(_object1),object2(_object2),
|
|
constraintrange(nc_range),featurerange(coord_range),
|
|
ee1index(_ee1index), ee2index(_ee2index)
|
|
{};
|
|
};
|
|
typedef std::map<std::string,ConstraintSet_struct*> ConstraintMap;
|
|
|
|
ObjectMap objects;
|
|
ConstraintMap constraints;
|
|
|
|
static bool getConstraintPose(ConstraintSet* constraint, void *_param, KDL::Frame& _pose);
|
|
};
|
|
|
|
}
|
|
|
|
#endif /* SCENE_HPP_ */
|