[SOLVED] How to use external data in SOFA ?

[luibass92]

Hi guys,

I’ve created an articulated chain (a hand) with 20 joints and I’ve added the meshes succesfully.

Then I’ve implemented a plugin in order to receive some data (joints rotations, a struct of 20 doubles) from an external server. The plugin seems to work good.
Now I would like to move the articulated chain using these data that I pass from the server in real-time, how can I do that?

I’ve created the chain using the class <ArticulatedHierarchyContainer /> but I’m just able to move the chain with SHIFT + LeftClick.

I think I have to do something like this:

<Node name="joints" activated="1">
      <joints name="jt" template="Vec3d"/>   //joints is the class implemented in my plugin
 </Node>
.
.
.
<ArticulatedHierarchyContainer J1="@../joints/jt.J1" J2="@../joints/jt.J2" ....  J20="@../joints/jt.J20" />
//J1...J20 are the joints values

but then I have no idea hot to use the data J1,J2,…,J20 to actually move the hand step-by-step.

I hope that someone can help me.
Cheers,
Luigi

[Hugo]

Hi Luigi

I am no expert in Rigid simulation.
However, there is an interesting example in /examples/Components/controller/ArticulatedHierarchyBVHController.scn.

What you need to do is to get inspired from it.

• create an interface component translating the information of your server into a data: which is a serie of joints Data<vector<BVHJoint> >),
• create a component inheriting from the ArticulatedHierarchyContainer and allowing to have as input a serie of joints (instead of a filename)
• finally connect both components

Cheers,

Hugo

linked to this ArticulatedHierarchyContainer.

[luibass92]

Hi Hugo, thanks for your reply.

I already know the example /examples/Components/controller/ArticulatedHierarchyBVHController.scn.
The main problem is that all the examples that use BVH Controller are based on the fact that they need the joints value of the complete simulation, then create a vector with all of these joints value and then move the simulated object (the hand in my case).

But I would like to move the hand in real time, step-by-step. So at each time step I don’t know the future of the simulation, I just know the current time step (and the previous ones).

I was wondering if exists a class through which I can just give a value to the joints rotation and apply it in the scene…

If you know someone expert in this kind of simulation could you give me a contact?

Cheers,
Luigi

[Hugo]

Hi Luigi,

I’ll try if some expert can shortly look at you problem. But for specific needs and support from experts, it then become a charged service of the SOFA Consortium.

At my best knowledge, there is no such a class. I wrote you the steps to follow for the implementation.

Best,

Hugo

[luibass92]

I’ve solved all the issues, the simulation works good!
I’ll post the code soon…

Cheers,
Luigi

[luibass92]

Hi everyone, I try to explain you the solution to my problem.

STEP 1.

I’ve created a Component of my plugin “Joints” that implements the socket communication (client side, supposing that you have already a working server that sends the values) here’s the code:

#define JOINTS_CPP
#include "joints.h"
#include <sofa/core/ObjectFactory.h>
#include <sofa/defaulttype/Vec3Types.h>

using namespace std;

typedef struct Giunti {
	double J1;
	.
        .//As many joints as you want
        .
	double J20;
}Giunti;

int socket_in=0;

namespace sofa
{

	namespace component
	{

		namespace behaviormodel
		{

			template <class DataTypes> 
			joints<DataTypes>::joints()
			:J1(initData(&J1,"J1", "Giunto 1"))
			,J2(initData(&J2,"J2", "Giunto 2"))
			.
                        .//As many joints as you want
                        .
			,J20(initData(&J20,"J20", "Giunto 20"))
			{	this->f_listening.setValue(true);
			}

			template <class DataTypes> 
			joints<DataTypes>::~joints()
			{
			}

			template <class DataTypes> 
			void joints<DataTypes>::init()
			{	
      			  std::cout << " init ok " << std::endl;
			}
			template <class DataTypes> 
			void joints<DataTypes>::reinit()
			{
			}

			template <class DataTypes> 
			void joints<DataTypes>::handleEvent(sofa::core::objectmodel::Event* event) { 
				
//Entra in questo "if" ciclicamente dal momento in cui si clicca su "Animate"
				if (dynamic_cast<simulation::AnimateBeginEvent*>(event)){  
					
					Giunti giunti_read;

					int r, bufflen = 160;
					char buf[bufflen];

					if(socket_in==0){

					socket_create_UDP_Recipient( socket_in, 9090 );
					cout << socket_in << endl;

					}	
					

					r = read( socket_in, buf, sizeof(buf) );
					

					cout << "letto: " << r << endl;
					giunti_read = *(( Giunti *)(buf));

					J1.setValue(giunti_read.J1);
					.
                                        . //As many joints as you want
                                        .
					J20.setValue(giunti_read.J20);
					
				}

			}

#ifndef SOFA_FLOAT
using sofa::defaulttype::Vec3dTypes;
#endif

#ifndef SOFA_DOUBLE
using sofa::defaulttype::Vec3fTypes;
#endif

SOFA_DECL_CLASS(joints)

int jointsClass = core::RegisterObject("jts")
#ifndef SOFA_FLOAT
        .add< joints<Vec3dTypes> >()
#endif
#ifndef SOFA_DOUBLE
        .add< joints<Vec3fTypes> >()
#endif
        ;
#ifndef SOFA_FLOAT
template class joints<Vec3dTypes>;
//template class EulerianFluidModel<Vec2dTypes>;
#endif
#ifndef SOFA_DOUBLE
template class joints<Vec3fTypes>;
//template class EulerianFluidModel<Vec2fTypes>;
#endif

		}	//behaviormodel

	}	//component

}	//sofa

STEP 2.

I’ve implemented two more components that, basically, are a split of the ArticulatedHierarchyContainer in two parts.
The first part (first component) is about the articulated hierarchy construction through the reading from a .bvh file (as the original ArtiulatedHierarchyContainer) but it doesn’t implement the motion part (because my aim is to move the hand not from file but in real-time from the values passed through socket).

The code of the first part (first component) is identical to the original ArticulatedHierarchyContainer.inl except for the buildCenterArticulationsTree function, that is reported here:

void ArticulatedHierarchyContainer::buildCenterArticulationsTree(sofa::helper::io::bvh::BVHJoint* bvhjoint, int id_buf, const char* name, simulation::Node* node)
{

    std::vector<sofa::helper::io::bvh::BVHJoint*> jointChildren = bvhjoint->getChildren();  //saves the jointChildren for the next iteration
    if (jointChildren.size()==0)
        return;

    std::string str(name);  //name of the current joint
    str.append("/");
    str.append(bvhjoint->getName());

    simulation::Node::SPtr nodeOfArticulationCenters =node->createChild(str);

    ArticulationCenter::SPtr ac = sofa::core::objectmodel::New<ArticulationCenter>();
    nodeOfArticulationCenters->addObject(ac);
    articulationCenters.push_back(ac.get());

    ac->posOnParent.setValue(Vector3(bvhjoint->getOffset()->x,bvhjoint->getOffset()->y,bvhjoint->getOffset()->z));
    ac->posOnChild.setValue(Vector3(0,0,0));
    ac->parentIndex.setValue(id_buf);
    ac->childIndex.setValue(bvhjoint->getId()+1);

    simulation::Node::SPtr nodeOfArticulations = nodeOfArticulationCenters->createChild("articulations");

    sofa::helper::io::bvh::BVHChannels* channels = bvhjoint->getChannels();  //Takes the channels of the current joint
    sofa::helper::io::bvh::BVHMotion* motion = bvhjoint->getMotion();

    serr<<"num Frames found in BVH ="<<motion->frameCount<<sendl;

    Articulation::SPtr a;

    for (unsigned int j=0; j<channels->channels.size(); j++)
    {
        switch(channels->channels[j])
        {
        case sofa::helper::io::bvh::BVHChannels::NOP:
            break;
        case sofa::helper::io::bvh::BVHChannels::Xposition:
            a = sofa::core::objectmodel::New<Articulation>();
            nodeOfArticulations->addObject(a);
            ac->articulations.push_back(a.get());
            a->axis.setValue(Vector3(1,0,0));
            a->translation.setValue(true);
            a->articulationIndex.setValue(id);
            //ALL THE MOTION PARTS ARE COMMENTED BECAUSE ARE IMPLEMENTED
            //IN THE SECOND PART (IN REAL-TIME)
            /*for (int k=0; k<motion->frameCount; k++)
                a->motion.push_back(motion->frames[k][j]);*/
            id++;
            break;
        case sofa::helper::io::bvh::BVHChannels::Yposition:
            a = sofa::core::objectmodel::New<Articulation>();
            nodeOfArticulations->addObject(a);
            ac->articulations.push_back(a.get());
            a->axis.setValue(Vector3(0,1,0));
            a->translation.setValue(true);
            a->articulationIndex.setValue(id);
            /*for (int k=0; k<motion->frameCount; k++)
                a->motion.push_back(motion->frames[k][j]);*/
            id++;
            break;
        case sofa::helper::io::bvh::BVHChannels::Zposition:
            a = sofa::core::objectmodel::New<Articulation>();
            nodeOfArticulations->addObject(a);
            ac->articulations.push_back(a.get());
            a->axis.setValue(Vector3(0,0,1));
            a->translation.setValue(true);
            a->articulationIndex.setValue(id);
            /*for (int k=0; k<motion->frameCount; k++)
                a->motion.push_back(motion->frames[k][j]);*/
            id++;
            break;
        case sofa::helper::io::bvh::BVHChannels::Xrotation:
            a = sofa::core::objectmodel::New<Articulation>();
            nodeOfArticulations->addObject(a);
            ac->articulations.push_back(a.get());
            a->axis.setValue(Vector3(1,0,0));
            a->rotation.setValue(true);
            a->articulationIndex.setValue(id);
            /*for (int k=0; k<motion->frameCount; k++)
                a->motion.push_back(motion->frames[k][j]);*/
            id++;
            break;
        case sofa::helper::io::bvh::BVHChannels::Yrotation:
            a = sofa::core::objectmodel::New<Articulation>();
            nodeOfArticulations->addObject(a);
            ac->articulations.push_back(a.get());
            a->axis.setValue(Vector3(0,1,0));
            a->rotation.setValue(true);
            a->articulationIndex.setValue(id);
            /*for (int k=0; k<motion->frameCount; k++)
                a->motion.push_back(motion->frames[k][j]);*/
            id++;
            break;
        case sofa::helper::io::bvh::BVHChannels::Zrotation:
            a = sofa::core::objectmodel::New<Articulation>();
            nodeOfArticulations->addObject(a);
            ac->articulations.push_back(a.get());
            a->axis.setValue(Vector3(0,0,1));
            a->rotation.setValue(true);
            a->articulationIndex.setValue(id);
            /*for (int k=0; k<motion->frameCount; k++)
                a->motion.push_back(motion->frames[k][j]);*/
            id++;
            break;
        }
    }

    for(unsigned int i=0; i<jointChildren.size(); i++)
    {
        buildCenterArticulationsTree(jointChildren[i], bvhjoint->getId()+1, bvhjoint->getName(), nodeOfArticulationCenters.get());
    }
}

The second part (second component) implements the motion of the joints in real time.
I’ve called it “ArticulatedHierarchyModifier”. Here is the code:

#ifndef SOFA_ArticulatedHierarchyModifier_INL
#define SOFA_ArticulatedHierarchyModifier_INL

#include "ArticulatedHierarchyModifier.h"
#include <sofa/core/visual/VisualParams.h>
#include <sofa/helper/system/FileRepository.h>
#include <sofa/simulation/common/Simulation.h>

namespace sofa
{

namespace component
{

namespace container
{

ArticulatedHierarchyModifier::ArticulatedHierarchyModifier():
    J1(initData(&J1,(double) 0,"J1", "Giunto 1"))
    ,J2(initData(&J2,(double) 0,"J2", "Giunto 2"))
    .
    . //as many joints as you want
    .
    ,J20(initData(&J20,(double) 0,"J20", "Giunto 20"))
{
this->f_listening.setValue(true);
    counter = 0;	//MODIFICATO
    jts.resize(0);
}

void ArticulatedHierarchyModifier::modifyArticulationsTree(sofa::helper::io::bvh::BVHJoint* bvhjoint)
{
    sofa::helper::io::bvh::BVHChannels* channels = bvhjoint->getChannels();  //Takes the current joint's channels

    sofa::helper::io::bvh::BVHMotion* motion = bvhjoint->getMotion();

   if (bvhjoint->getChildren().size()==0){
        return;
	}

	std::vector<double> angles;
	angles.resize(0);
	for(int i=0; i<channels->channels.size(); i++){
		angles.push_back(jts[counter]);
		counter++;
	}

	
	std::vector< std::vector< double > > Angles;
	Angles.resize(0, vector<double>(0));
	Angles.push_back(angles);  //Angles[0] is the vector angles

	motion->frameCount = 1;   //Because we are in real-time, so at each time step
                                  //there is just one fram
	motion->frameTime = 1;
	motion->frames = Angles;

    Articulation::SPtr a;

        vector<ArticulationCenter*> artcenters = ArtContainer->getArticulationCenters();
        ArticulationCenter::SPtr ac0;

        vector<ArticulationCenter*>::const_iterator ac = artcenters.begin();
        vector<ArticulationCenter*>::const_iterator acEnd = artcenters.end();
        for (; ac != acEnd; ac++)
        {

	if ((*ac)->childIndex.getValue() == bvhjoint->getId()+1)
 		ac0 = (*ac);
	} 

    for (unsigned int j=0; j<channels->channels.size(); j++)
    {
	a = ac0->articulations[j];
	a->motion.resize(0);
    for (int k=0; k<motion->frameCount; k++)
    a->motion.push_back(motion->frames[k][j]);
    }

    for(unsigned int i=0; i<bvhjoint->getChildren().size(); i++)
    {
        modifyArticulationsTree(bvhjoint->getChildren()[i]);
    }

}

void ArticulatedHierarchyModifier::init ()
{
     std::cout << "Init Modifier" << std::endl;
}

void ArticulatedHierarchyModifier::handleEvent(sofa::core::objectmodel::Event* event) {

if (dynamic_cast<simulation::AnimateBeginEvent*>(event)){  

counter = 0;

   jts.resize(0);

for(int i=0; i<6; i++)
    		jts.push_back(0);   //In my personal configuration I need the first
                                    // 6 joints always 0...

    jts.push_back(J1.getValue());
    .
    .//as many joints as you want
    .
    jts.push_back(J20.getValue());

    simulation::Node* context = dynamic_cast<simulation::Node *>(this->getContext()); // access to current node
    context->get<sofa::component::container::ArticulatedHierarchyContainer>(ArtContainer);

modifyArticulationsTree(ArtContainer->getJoint());

}

} 

} // namespace container

} // namespace component

} // namespace sofa

#endif

STEP 3.

Add the plugin to your scene and instead of the original “ArticulatedHierarchyContainer” insert the new component followed by ArticulatedHierarchyModifier. Of course you’ll need an ArticulatedHierarchyBVHController too.

The code is tested and seems to work good.
Hope this can help someone! I mark this topic as resolved.

Cheers,
Luigi

[Hugo]

Thank you VERY much for sharing this Luigi!
Cheers

Hugo

[Author]

Posts