Fix [#31544]: iTaSC assertion when creating armature with no joint. This degenerated case can be obtained by having a single bone in the IK chain and locking all 3 axis. This case was causing an assert in the KDL library. The bug is fixed by simply not creating the IK scene in this case.

intern/itasc/Armature.cpp

@@ -369,11 +369,13 @@ int Armature::addEndEffector(const std::string& name)
 	return m_neffector++;
 }
 
-void Armature::finalize()
+bool Armature::finalize()
 {
 	unsigned int i, j, c;
 	if (m_finalized)
-		return;
+		return true;
+	if (m_njoint == 0)
+		return false;
 	initialize(m_njoint, m_noutput, m_neffector);
 	for (i=c=0; i<m_nconstraint; i++) {
 		JointConstraint_struct* pConstraint = m_constraints[i];
@@ -410,6 +412,7 @@ void Armature::finalize()
 	if (m_armlength < KDL::epsilon)
 		m_armlength = KDL::epsilon;
 	m_finalized = true;
+	return true;
 }
 
 void Armature::pushCache(const Timestamp& timestamp)

intern/itasc/Armature.hpp

@@ -31,7 +31,7 @@ public:
 	bool getSegment(const std::string& segment_name, const unsigned int q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip);
 	bool getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name=m_root);
 
-	virtual void finalize();
+	virtual bool finalize();
 
 	virtual int addEndEffector(const std::string& name);
 	virtual const Frame& getPose(const unsigned int end_effector);

intern/itasc/FixedObject.cpp

@@ -53,12 +53,13 @@ int FixedObject::addEndEffector(const std::string& name)
 	return -1;
 }
 
-void FixedObject::finalize()
+bool FixedObject::finalize()
 {
 	if (m_finalized)
-		return;
+		return true;
 	initialize(0, m_nframe);
 	m_finalized = true;
+	return true;
 }
 
 const Frame& FixedObject::getPose(const unsigned int frameIndex)

intern/itasc/FixedObject.hpp

@@ -23,7 +23,7 @@ public:
 
 	virtual void updateCoordinates(const Timestamp& timestamp) {};
 	virtual int addEndEffector(const std::string& name);
-	virtual void finalize();
+	virtual bool finalize();
 	virtual const Frame& getPose(const unsigned int frameIndex);
 	virtual void updateKinematics(const Timestamp& timestamp) {};
 	virtual void pushCache(const Timestamp& timestamp) {};

intern/itasc/MovingFrame.cpp

@@ -27,9 +27,10 @@ MovingFrame::~MovingFrame()
 {
 }
 
-void MovingFrame::finalize()
+bool MovingFrame::finalize()
 {
 	updateJacobian();
+	return true;
 }
 
 void MovingFrame::initCache(Cache *_cache)

intern/itasc/MovingFrame.hpp

@@ -28,7 +28,7 @@ public:
 	virtual void updateKinematics(const Timestamp& timestamp);
     virtual void pushCache(const Timestamp& timestamp);
 	virtual void initCache(Cache *_cache);
-	virtual void finalize();
+	virtual bool finalize();
 protected:
 	virtual void updateJacobian();
 

intern/itasc/Object.hpp

@@ -33,7 +33,7 @@ public:
     virtual ~Object(){};
 
 	virtual int addEndEffector(const std::string& name){return 0;};
-	virtual void finalize(){};
+	virtual bool finalize(){return true;};
 	virtual const KDL::Frame& getPose(const unsigned int end_effector=0){return m_internalPose;};
     virtual const ObjectType getType(){return m_type;};
     virtual const unsigned int getNrOfCoordinates(){return 0;};

intern/itasc/Scene.cpp

@@ -91,7 +91,8 @@ bool Scene::setParam(SceneParam paramId, double value)
 bool Scene::addObject(const std::string& name, Object* object, UncontrolledObject* base, const std::string& baseFrame)
 {
 	// finalize the object before adding
-	object->finalize();
+	if (!object->finalize())
+		return false;
     //Check if Object is controlled or uncontrolled.
     if(object->getType()==Object::Controlled){
 		int baseFrameIndex = base->addEndEffector(baseFrame);