[luibass92]

Hi everyone,

In these days I’ve been a visitor at INRIA Lille!!
It has been a really great experience and the guys there, in particular Christian and Eulalie, helped me a lot with my project.

The problem of the wrong collision detection was due to the fact that I’m using a global approach instead of a local one.
So the solution, as Eulalie suggested is to use the following classes in the root node:

<FreeMotionAnimationLoop />
<CollisionPipeline draw ="0" verbose ="0"/>
<BruteForceDetection name ="N2"/>
<LocalMinDistance name ="Proximity" contactDistance ="0.05" alarmDistance ="0.5"/>
<CollisionResponse name ="Response" response ="FrictionContact"/>
<CollisionGroup name ="Group"/>
<LCPConstraintSolver maxIt ="1000" tolerance ="0.000001"/>

<!-- Note that you will need the SOFA_BUILD_METIS option in the configuration for using these sparse solvers -->

And then, when the articulation node is opened:

<Node name =" articulationHand ">
<EulerImplicit name ="cg odesolver" printLog ="false"/>
<CGLinearSolver name ="linear solver" tolerance ="1e -20" threshold ="1e -20" iterations="10"/>
<MechanicalObject name ="Articulations" position ="0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.2617 0 0 0 0" template ="Vec1d"/>
<RestShapeSpringsForceField points ="0 1 2 3 4 5" stiffness ="100000000000"/>
<RestShapeSpringsForceField stiffness ="100000"/>
<UncoupledConstraintCorrection compliance ="0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0"/>

In this way everything goes fine and the collision is done correctly!

Cheers and thanks a lot everyone for helping me!
I’ll finally mark this topic as solved 🙂

Luigi

[luibass92]

Actually my original question was different, so this topic is not solved… Your tip helped me to soolve a sub-issue of my problem 🙂

Best,
Luigi

[luibass92]

Hey Hugo,

with your tip showObject="1" it works fine, thanks!
In next days I will try to modify my scene with these components and I will let you know what happens.

Cheers,
Luigi

[luibass92]

Hey Hugo,

No, I don’t have any OglModel in the scene, but as you said I should see at least the collision models (yes, I’ve activated the Collision>Collision Models in the SOFA gui).

Cheers,
Luigi

[luibass92]

Adding the SOFA_LIB_COMPONENT_SPARSE_SOLVER in the configurations it doesn’t crash anymore, but I don’t visualize anything in the scene…

[luibass92]

I still get the errors:

WARNING[Cube1(GNode)]: Object type "SparseLUSolver" creation Failed
WARNING[Cube2(GNode)]: Object type "SparseLUSolver" creation Failed
LOAD ERROR: Node initialization failed.
WARNING[Cube1(GNode)]: ERROR:  requires a LinearSolver.
WARNING[Cube1(GNode)]: ERROR:  requires a LinearSolver.
WARNING[Cube1(GNode)]: ERROR:  requires a LinearSolver.
WARNING[Cube1(GNode)]: ERROR:  requires a LinearSolver.
WARNING[Cube2(GNode)]: ERROR:  requires a LinearSolver.
WARNING[Cube2(GNode)]: ERROR:  requires a LinearSolver.
WARNING[Cube2(GNode)]: ERROR:  requires a LinearSolver.
WARNING[Cube2(GNode)]: ERROR:  requires a LinearSolver.

[luibass92]

Hi Eulalie and thanks for your reply!

I’ve already used the three model together (Triangle, Line and Point)…

I’m trying to run the scene that you posted but I get the following errors:

WARNING[Cube1(GNode)]: Object type "SparseLDLSolver" creation Failed
WARNING[Cube2(GNode)]: Object type "SparseLDLSolver" creation Failed
LOAD ERROR: Node initialization failed.
WARNING[Cube1(GNode)]: ERROR:  requires a LinearSolver.
WARNING[Cube1(GNode)]: ERROR:  requires a LinearSolver.
WARNING[Cube1(GNode)]: ERROR:  requires a LinearSolver.
WARNING[Cube1(GNode)]: ERROR:  requires a LinearSolver.
WARNING[Cube2(GNode)]: ERROR:  requires a LinearSolver.
WARNING[Cube2(GNode)]: ERROR:  requires a LinearSolver.
WARNING[Cube2(GNode)]: ERROR:  requires a LinearSolver.
WARNING[Cube2(GNode)]: ERROR:  requires a LinearSolver.

I don’t know why but it fails to create the “SparseLDLSolver” 🙁 .

Cheers,
Luigi

[luibass92]

Hi Hugo,

I’m still in troubles, I would like to try a different <CollisionResponse /> method.
In my scene I’m using <CollisionResponse name="response" response="default" /> but I think I would have better results if I use <CollisionResponse name="response" response="FrictionContact" /> (I’ve seen this in some default SOFA’s scenes).
Unfortunately when I try this solution my scene crashes as soon as the hand collides with the sphere…
Do you know if it’s normal that FrictionContact makes the scene crash?

Cheers,
Luigi

[luibass92]

Hi Hugo,

yes the collision is detected but the problem is that the sphere doesn’t move nor deform, so the finger passes through the sphere as if the interaction doesn’t happen.

I’m pretty sure that alarm and contact distance are correct, I have also tried to change them to different values (higher or lower) but doesn’t change anything.

My collision pipeline and contact manager are the following (in the root node):

   <CollisionPipeline verbose="0" draw="0"/>
    <BruteForceDetection name="N2" />
    <NewProximityIntersection name="Proximity" alarmDistance="0.5" contactDistance="0.3" />
    <CollisionResponse name="Response" response="default" />
    <CollisionGroup name="Group" />
    <LCPConstraintSolver tolerance="0.001" maxIterations="1000"/>

Then I have in the node of the hand and in the node of the sphere the following solvers:

		<EulerImplicit name="cg odesolver" printLog="false" />
        	<CGLinearSolver iterations="100" name="linear solver" threshold="1e-20" tolerance="1e-20" />

(I have tried also putting these solvers in the root node, but it’s the same).

I don’t know if it’s an issue of time steps / stability, it could be… But I have no idea how to solve this kind of issues.

Thanks for your replies!

Cheers,
Luigi

[luibass92]

I have not found a solution yet…
Does anyone have figured out something ?

Cheers,
Luigi

[luibass92]

Hi Hugo,

Yes, the hand is always the same 🙂
I’ve already checked the normals of the fingers and they seem OK (they go outside of the mesh).
I’ve also put the sphere and the fingers in different groups: all the hand’s meshes are group 1 and the sphere is group 2…

If it can help I post a portion of the code:

<Node name="articulationHand">
            <MechanicalObject name="Articulations" template="Vec1d" position="0 0 0 0" />
            <Node>
		<EulerImplicit name="cg odesolver" printLog="false" />
        	<CGLinearSolver iterations="100" name="linear solver" threshold="1e-20" tolerance="1e-20" />
                <MechanicalObject template="Rigid" name="DOFs" position="0 0 0				0 0 0 1

									 0 11 -0.6  			0 0 0 1
									 0 16.004 -0.6  		0 0 0 1
.
.
. <!--The same for each frame of the hand-->
." /> 

                <UniformMass template="Rigid" name="mass" mass="0.1 0.1 [1 0 0,0 1 0,0 0 1]" />
                <FixedConstraint template="Rigid" name="fixOrigin" indices="0" />
                <ArticulatedSystemMapping input1="@../Articulations" output="@DOFs" />
		

<!--######################### PALM #########################-->
		<Node name="Collision" activated="1">
                    <MeshObjLoader name="MeshLoader"  filename="mesh/h10d.obj" scale="100" translation="0 0 0.7" rotation="0 180 0" />
		    <Mesh src="@MeshLoader" /> 
                    <MechanicalObject template="Vec3d" src="@MeshLoader" />
                    <TriangleModel group="1"/>
                    <LineModel group="1"/>
                    <RigidMapping  input="@.." index="0"/>
                </Node>
                <Node name="Visu" activated="1">
                    <OglModel name="Visual" filename="mesh/h10s.obj" scale="100" translation="0 0 0.7" rotation="0 180 0"/>
                    <RigidMapping template="Rigid3d,ExtVec3f" input="@.." index="0" output="@Visual" />
               </Node>
.
.
. <!-- The same for each mesh of the hand -->
.
.

</Node>

             <ArticulatedHierarchyContainer filename="BehaviorModels/schunkHand3.bvh"/>
	     
<!--MIDDLE: J1~J3
    INDEX: J4~J7
    PALM(two identical joints): J8 = J8_2
    RING: J9~J12
    LITTLE: J13~J16
    THUMB J17~J20	(Jdummy used for the thumb orientation)-->

	     <ArticulatedHierarchyModifier J1="0"
					   J2="0"
                                           .
                                           .
                                           .<!--The same for each Joint (sent through socket)-->
                                           .
                                           .
/>
             <ArticulatedHierarchyBVHController />

</Node> <!--articulationHand-->

<!--################# SPHERE ########################-->
                <Node name= "sphere">
			<EulerImplicit name="cg odesolver" printLog="false" />
        		<CGLinearSolver iterations="100" name="linear solver" threshold="1e-20" tolerance="1e-20" />
			<MeshVTKLoader name="MeshLoader"  filename="mesh/sphere10.vtu" />
			<Mesh src="@MeshLoader" /> 
        		<TetrahedronSetTopologyContainer name="topo" src="@MeshLoader" />
        		<TetrahedronSetGeometryAlgorithms drawEdges="0" />
      			<MechanicalObject name="dofs" src="@MeshLoader" rotation="45 0 0" translation="0.0 10 7.5" scale = "100"/>
			<UniformMass mass="0.2"/>
			<TetrahedronFEMForceField name="FEM" youngModulus="5000" poissonRatio="0.3" method="large" />
			<Node name="sourceSurface">
          			<MeshObjLoader name="Surface" filename="mesh/sphere.obj" />
         			<Mesh src="@Surface" />
        			<MechanicalObject name="surf" position = "@[-1].position" template="Vec3d" rotation="45 0 0" translation="0.0 10 7.5" scale = "100" />
 				<TriangleSetGeometryAlgorithms template="Vec3d" name="Geometry Algorithms"/>
				<TriangleModel group="2"/>
            			<BarycentricMapping />
 			</Node>
            	</Node>

“ArticulatedHierarchyModifier” is a component created by me that just modifies the joints value of the articulated chain step-by-step reading them from socket (it works good).

I have the suspect that the issue is due to the fact that I pass the joints values through socket and SOFA refreshes them at each time step, in this case SOFA refreshes always “0”, so the hand apparently doesn’t move, but virtually it changes its position from “0” to “0” (again) at each time step and this causes some problems to the collision detection…

EDIT
This is a short gif animation that shows the issue:

GIF

Cheers,
Luigi

[luibass92]

Sure, I’ll mark it as solved!

Cheers,
Luigi

[luibass92]

Hi Hugo,

I’ve decimated all the meshes (some with a ratio of 0.25, others with 0.5) and now the simulation without other objects in the scene runs at 30 FPS more or less.
Obviously if I add another object (a sphere) the simulation slow down to less than 10 FPS.

Anyway your tips have been really useful, thanks a lot!

I will take a look to the CUDA plugin for sure.

Cheers,
Luigi

[luibass92]

Hi Hugo,

I will try the decimation of the meshes soon! Thanks for the tip.

What do you mean with “GPU collision detection”? I run my entire system using my GPU NVIDIA GeForce GT 520MX, I’ve installed the drivers and now it is used just the NVIDIA as GPU for every operation…
Is there a way to tell SOFA to use the NVIDIA drive just for collision detection?

Cheers,
Luigi

[luibass92]

OK these are the screenshots of my simulation, respectively collision meshes and visual meshes (they are identical):

The meshes are not created by me, but downloaded from the web. The hand constructor doesn’t provide official meshes but I’m sure that these ones are correct.
So I can’t tell the estimation of the number of points… But zooming in the meshes I can see that they are “empty” inside, so there are no useless points, there are just the superficial points.

If it is useful my code for generating the meshes is always the same (repeated for each mesh):

<Node name="Collision">
                    <MeshObjLoader name="MeshLoader"  filename="mesh/f21.obj" scale="100" rotation="180 0 90"/>
		    <Mesh src="@MeshLoader" /> 
                    <MechanicalObject template="Vec3d" src="@MeshLoader"  />
                    <TriangleModel group="1"/>
                    <LineModel group="1"/>
                    <RigidMapping  input="@.." index="2" />
                </Node>
<Node name="Visu">
                    <OglModel name="Visual" filename="mesh/f21.obj" scale="100" rotation="180 0 90"/>
                    <RigidMapping template="Rigid3d,ExtVec3f" input="@.." index="2" output="@Visual" />
</Node>

I don’t know if there are attributes to change in the code (in LineModel and TriangleModel for example).

I didn’t try to remove all the collision meshes because I know that I’ll need them in my project (the hand should manipulate a deformable object in the scene). I tried to remove all the viual meshes (they are not so useful) but it didn’t change anything…

Thanks again for your reply,
Luigi

EDIT: I’ve tried to deactivate all the collision meshes nodes and actually the simulation goes way faster… So the problem are these collision meshes even if I put them all in the same group (<LineModel group="1" />)

EDIT 2: I found out that my meshes were constructed with a lot of duplicated points (I don’t know why), so I’ve simplified them and now the simulation is faster. Unfortunately if I add another object in the scene activating the collision detection with the hand, it goes still slow. So if you have some other suggestions to improve the speed of my simulation I’ll thank you :).

[luibass92]

Hi guys,

First of all thanks for your replies.

My application is a robotic hand, the meshes are included and are 20 meshes (collision and visual) for 20 DoF in total… I know it is a lot but the collision detection is not actually working because the hand is the only object in the scene and auto-collisions are not modeled (all of the meshes have the same “group” attribute).

The hand should move according to its joints values passed through socket (it works I’ve tested more times).
The problem is that the joints values are calculated starting from the sensors mounted on the real robotic hand that I have in my lab.
So I would like to see the SOFA simulated hand moving in real time with the real robotic hand and not with 3/4 seconds of delay. This is why I need 30 or higher FPS…

I tried the 25 iterations and 1-e6 tolerance but didn’t change anything.

Hugo what do you mean with “reducing the resolution of your objects (mechanical, visual, collision)”? I don’t think I’ve set the resolutions…

Cheers,
Luigi

[luibass92]

In my scene I have always 20 FPS more or less.
Do you know some ways to increase the FPS rate? I would like to reach 50-60 FPS.

I’m using as graphic drive a NVIDIA GT 520MX (1 GB).

As solvers I’m using the following:

<EulerImplicit name="cg odesolver" printLog="false" />
<CGLinearSolver iterations="100" name="linear solver" threshold="1e-20" tolerance="1e-20" />

I don’t know if they can cause the low FPS rate…

Cheers,
Luigi

[luibass92]

Thanks for your reply Hugo.

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

[luibass92]

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

Cheers,
Luigi

[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

[luibass92]

OK, this is the code that works.

template <class DataTypes> 
			void joints<DataTypes>::handleEvent(sofa::core::objectmodel::Event* event) { 
				

	if (dynamic_cast<simulation::AnimateBeginEvent*>(event)){  
					
		Joints giunti_read;  //This is a struct made of 20 doubles ("giunti" means
                                     //"joints" in italian :D )

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

		if(socket_in==0){  //This if solved the issue, now the socket is created
                                   //just one time

		socket_create_UDP_Recipient( socket_in, 9090 ); //This is a typical client
		cout << socket_in << endl;

		}	
					

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

		cout << "read: " << r << endl;
		giunti_read = *(( Joints *)(buf));

	        J1.setValue(giunti_read.J1);
		J2.setValue(giunti_read.J2);
		J3.setValue(giunti_read.J3);
		J4.setValue(giunti_read.J4);
		J5.setValue(giunti_read.J5);
		J6.setValue(giunti_read.J6);
		J7.setValue(giunti_read.J7);
		J8.setValue(giunti_read.J8);
		J9.setValue(giunti_read.J9);
	        J10.setValue(giunti_read.J10);
	        J11.setValue(giunti_read.J11);
		J12.setValue(giunti_read.J12);
		J13.setValue(giunti_read.J13);
		J14.setValue(giunti_read.J14);
		J15.setValue(giunti_read.J15);
		J16.setValue(giunti_read.J16);
		J17.setValue(giunti_read.J17);
		J18.setValue(giunti_read.J18);
		J19.setValue(giunti_read.J19);
		J20.setValue(giunti_read.J20);
					

		cout << " J1 = " << J1 << endl;
		cout << " J2 = " << J2 << endl;
		cout << " J3 = " << J3 << endl;
		cout << " J4 = " << J4 << endl;
		cout << " J5 = " << J5 << endl;
		cout << " J6 = " << J6 << endl;
		cout << " J7 = " << J7 << endl;
		cout << " J8 = " << J8 << endl;
		cout << " J9 = " << J9 << endl;
		cout << " J10 = " << J10 << endl;
		cout << " J11 = " << J11 << endl;
		cout << " J12 = " << J12 << endl;
		cout << " J13 = " << J13 << endl;
		cout << " J14 = " << J14 << endl;
		cout << " J15 = " << J15 << endl;
		cout << " J16 = " << J16 << endl;
		cout << " J17 = " << J17 << endl;
		cout << " J18 = " << J18 << endl;
		cout << " J19 = " << J19 << endl;
		cout << " J20 = " << J20 << endl;
		cout << endl;
	}

Note that this plugins works only if the server is already in running mode. Otherwise crashes.

Hugo and Guillaume I’ve posted another question always about this project, it’s the final part and I don’t know how to use these data J1…J20 in my scene. If you read can you help me please (this is the post https://www.sofa-framework.org/community/forum/topic/how-to-use-external-data-in-sofa/)?

Thanks and cheers,
Luigi

[luibass92]

Hey Guillaume,

I’ve solved this issue, I don’t know if I should post the code here because it’s quite long…
Anyway the problem is caused by the fact that in my .ccp file I have:

void joints<DataTypes>::handleEvent(sofa::core::objectmodel::Event* event) { 

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

           // All the code here for the communication through socket (client side)

       }
}

that is inside a “big loop” since the moment I click on the Animate button.
So the software crashes because it tries to recreate for each loop a socket for the communication, so I just added an “if” statement that imply the creation of the socket just one time.
In this way it runs and the communication works if I run the server first.

I don’t know why, if I try to run the plugin before it continue to crash…

Cheers,
Luigi

[luibass92]

Yes, I think you are right. Anyway the problem is caused by this part of the code:

<Node name="joints">
      <joints name="jt" template="Vec3d"/>
 </Node>

(joints is the new name of MyClass)

I’ve tried to comment it and the simulation doesn’t crash anymore.
In particular in joints.cpp I found out that the function:

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

is the problem (I can’t figure out why)…

The complete code of the joints.cpp file is the following (it is just a client):

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

using namespace std;

typedef struct Giunti {
	double q1;
}Giunti;

namespace sofa
{

	namespace component
	{

		namespace behaviormodel
		{

			template <class DataTypes> 
			joints<DataTypes>::joints()
			:q1(initData(&q1,"q1", "Giunto 1"))
			{	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) { 

				if (dynamic_cast<simulation::AnimateBeginEvent*>(event))
					{
		
					int t = (int)this->getContext()->getTime();

					int socket_in;
					listener_socket( 9090, &socket_in );
					Giunti giunti_read;

					int r, bufflen = 16;
					char buf[bufflen];
					int slen, rlen;
					sockaddr_in si_other;

					r = read( socket_in, buf, sizeof(buf) );  //-----> crash SOFA
					cout << "letto: " << r << endl;
					giunti_read = *(( Giunti *)(buf));

					q1.setValue(giunti_read.q1);
	
					cout << " q1 = " << q1 << endl;

				}

			}

#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

[luibass92]

OK, I tried adding to each <TriangleModel/> and <LineModel/> the same “group” and the simulation goes way faster than before!

Thanks a lot for your help!

Cheers,
Luigi

[Author]

Posts