Skip to content

Commit

Permalink
Merge pull request #30 from EulalieCoevoet/pr_accessconstraintindex
Browse files Browse the repository at this point in the history 
[constraint] Access constraints' index
  • Loading branch information
@EulalieCoevoet
EulalieCoevoet authored Jun 4, 2024
2 parents f50e0c5 + 9db2b14 commit c82eb54
Show file tree
Hide file tree
Showing 25 changed files with 77 additions and 64 deletions.
1 change: 1 addition & 0 deletions src/SoftRobots.Inverse/binding/Binding_QPInverseProblemSolver_doc.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ static auto QPInverseProblemSolver = R"(
Typical usage example:
import Sofa.Constraint
import Sofa.SoftRobotsInverse
Expand Down
11 changes: 6 additions & 5 deletions src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,8 @@ void BarycentricCenterEffector<Rigid3Types>::buildConstraintMatrix(const Constra
SOFA_UNUSED(cParams);
SOFA_UNUSED(x);

m_constraintId = cIndex;
m_constraintIndex.setValue(cIndex);
const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex);

const unsigned int nbp = m_state->getSize();

Expand All @@ -60,23 +61,23 @@ void BarycentricCenterEffector<Rigid3Types>::buildConstraintMatrix(const Constra

if(d_axis.getValue()[0])
{
MatrixDerivRowIterator rowIterator = matrix.writeLine(m_constraintId+index);
MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex+index);
for (unsigned int i=0; i<nbp; i++)
rowIterator.setCol(i, Deriv(cx,vZero));
index++;
}

if(d_axis.getValue()[1])
{
MatrixDerivRowIterator rowIterator = matrix.writeLine(m_constraintId+index);
MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex+index);
for (unsigned int i=0; i<nbp; i++)
rowIterator.setCol(i, Deriv(cy,vZero));
index++;
}

if(d_axis.getValue()[2])
{
MatrixDerivRowIterator rowIterator = matrix.writeLine(m_constraintId+index);
MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex+index);
for (unsigned int i=0; i<nbp; i++)
rowIterator.setCol(i, Deriv(cz,vZero));
index++;
Expand All @@ -85,7 +86,7 @@ void BarycentricCenterEffector<Rigid3Types>::buildConstraintMatrix(const Constra
cIndex+=index;

cMatrix.endEdit();
m_nbLines = cIndex - m_constraintId;
m_nbLines = cIndex - constraintIndex;
}


Expand Down
2 changes: 1 addition & 1 deletion src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,7 @@ class BarycentricCenterEffector : public Effector<DataTypes>
using Effector<DataTypes>::m_state ;
using Effector<DataTypes>::getContext ;
using Effector<DataTypes>::m_nbLines ;
using Effector<DataTypes>::m_constraintId ;
using Effector<DataTypes>::m_constraintIndex ;
using Effector<DataTypes>::f_listening ;
using Effector<DataTypes>::getTarget ;

Expand Down
19 changes: 11 additions & 8 deletions src/SoftRobots.Inverse/component/constraint/BarycentricCenterEffector.inl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,7 +144,8 @@ void BarycentricCenterEffector<DataTypes>::buildConstraintMatrix(const Constrain
SOFA_UNUSED(cParams);
SOFA_UNUSED(x);

m_constraintId = cIndex;
m_constraintIndex.setValue(cIndex);
const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex);

const unsigned int nbp = m_state->getSize();

Expand All @@ -154,23 +155,23 @@ void BarycentricCenterEffector<DataTypes>::buildConstraintMatrix(const Constrain

if(d_axis.getValue()[0])
{
MatrixDerivRowIterator rowIterator = matrix.writeLine(m_constraintId+index);
MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex+index);
for (unsigned int i=0; i<nbp; i++)
rowIterator.setCol(i, Deriv(1.0/Real(nbp), 0, 0));
index++;
}

if(d_axis.getValue()[1])
{
MatrixDerivRowIterator rowIterator = matrix.writeLine(m_constraintId+index);
MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex+index);
for (unsigned int i=0; i<nbp; i++)
rowIterator.setCol(i, Deriv(0, 1.0/Real(nbp), 0));
index++;
}

if(d_axis.getValue()[2])
{
MatrixDerivRowIterator rowIterator = matrix.writeLine(m_constraintId+index);
MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex+index);
for (unsigned int i=0; i<nbp; i++)
rowIterator.setCol(i, Deriv(0, 0, 1.0/Real(nbp)));
index++;
Expand All @@ -179,7 +180,7 @@ void BarycentricCenterEffector<DataTypes>::buildConstraintMatrix(const Constrain
cIndex+=index;

cMatrix.endEdit();
m_nbLines = cIndex - m_constraintId;
m_nbLines = cIndex - constraintIndex;

computeBarycenter();
}
Expand Down Expand Up @@ -207,22 +208,24 @@ void BarycentricCenterEffector<DataTypes>::getConstraintViolation(const Constrai
for(unsigned int i=0; i<m_nbLines; i++)
dFree[i] += Jdx->element(i);

const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex);

int index = 0;
if(d_axis.getValue()[0])
{
resV->set(m_constraintId, dFree[0]);
resV->set(constraintIndex, dFree[0]);
index++;
}

if(d_axis.getValue()[1])
{
resV->set(m_constraintId+index, dFree[1]);
resV->set(constraintIndex+index, dFree[1]);
index++;
}

if(d_axis.getValue()[2])
{
resV->set(m_constraintId+index, dFree[2]);
resV->set(constraintIndex+index, dFree[2]);
index++;
}
}
Expand Down
2 changes: 1 addition & 1 deletion src/SoftRobots.Inverse/component/constraint/CableEffector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ class CableEffector : public softrobotsinverse::behavior::Effector<DataTypes> ,
/// otherwise any access to the base::attribute would require
/// the "this->" approach.
using softrobots::constraint::CableModel<DataTypes>::d_cableLength ;
using softrobots::constraint::CableModel<DataTypes>::m_constraintId ;
using softrobots::constraint::CableModel<DataTypes>::m_constraintIndex ;
using softrobots::constraint::CableModel<DataTypes>::m_state ;
using softrobots::constraint::CableModel<DataTypes>::getCableLength ;

Expand Down
2 changes: 1 addition & 1 deletion src/SoftRobots.Inverse/component/constraint/CableEffector.inl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,7 @@ void CableEffector<DataTypes>::getConstraintViolation(const ConstraintParams* cP
d_cableLength.setValue(getCableLength(m_state->readPositions().ref()));
Real desiredLength = getTarget(d_desiredLength.getValue(), d_cableLength.getValue());
Real dfree = Jdx->element(0) + desiredLength - d_cableLength.getValue();
resV->set(m_constraintId, dfree);
resV->set(m_constraintIndex.getValue(), dfree);
}

} // namespace
2 changes: 1 addition & 1 deletion src/SoftRobots.Inverse/component/constraint/ForcePointActuator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ class ForcePointActuator : public Actuator<DataTypes>
/// otherwise any access to the base::attribute would require
/// the "this->" approach.
using Actuator<DataTypes>::m_state ;
using Actuator<DataTypes>::m_constraintId ;
using Actuator<DataTypes>::m_constraintIndex ;
////////////////////////////////////////////////////////////////////////////


Expand Down
16 changes: 9 additions & 7 deletions src/SoftRobots.Inverse/component/constraint/ForcePointActuator.inl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -212,7 +212,8 @@ void ForcePointActuator<DataTypes>::buildConstraintMatrix(const ConstraintParams
SOFA_UNUSED(cParams);
SOFA_UNUSED(x);

m_constraintId = cIndex;
m_constraintIndex.setValue(cIndex);
const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex);

Deriv direction = d_direction.getValue();

Expand All @@ -224,7 +225,7 @@ void ForcePointActuator<DataTypes>::buildConstraintMatrix(const ConstraintParams
{
Deriv dir;
dir[j] = 1;
MatrixDerivRowIterator rowIterator = matrix.writeLine(m_constraintId+j);
MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex+j);
for(unsigned int i=0; i<d_indices.getValue().size(); i++)
if(d_indices.getValue()[i]<m_state->getSize())
rowIterator.addCol(d_indices.getValue()[i], dir);
Expand All @@ -235,7 +236,7 @@ void ForcePointActuator<DataTypes>::buildConstraintMatrix(const ConstraintParams
{
direction /= direction.norm();
MatrixDeriv& matrix = *cMatrix.beginEdit();
MatrixDerivRowIterator rowIterator = matrix.writeLine(m_constraintId);
MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex);
for(unsigned int i=0; i<d_indices.getValue().size(); i++)
if(d_indices.getValue()[i]<m_state->getSize())
rowIterator.addCol(d_indices.getValue()[i], direction);
Expand All @@ -244,8 +245,8 @@ void ForcePointActuator<DataTypes>::buildConstraintMatrix(const ConstraintParams
}

cMatrix.endEdit();

m_nbLines = cIndex - m_constraintId;
m_nbLines = cIndex - constraintIndex;
}


Expand All @@ -257,15 +258,16 @@ void ForcePointActuator<DataTypes>::getConstraintViolation(const ConstraintParam
SOFA_UNUSED(cParams);
SOFA_UNUSED(Jdx);

const auto& constraintId = sofa::helper::getReadAccessor(m_constraintIndex);
Deriv direction = d_direction.getValue();

if(direction.norm() == 0) // No fixed direction
{
for(unsigned int j=0; j<Deriv::total_size; j++)
resV->set(m_constraintId+j, 0.);
resV->set(constraintId+j, 0.);
}
else
resV->set(m_constraintId, 0.);
resV->set(constraintId, 0.);
}


Expand Down
2 changes: 1 addition & 1 deletion src/SoftRobots.Inverse/component/constraint/ForceSurfaceActuator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,7 @@ class ForceSurfaceActuator : public Actuator<DataTypes>
/// otherwise any access to the base::attribute would require
/// the "this->" approach.
using Actuator<DataTypes>::m_state ;
using Actuator<DataTypes>::m_constraintId ;
using Actuator<DataTypes>::m_constraintIndex ;
////////////////////////////////////////////////////////////////////////////


Expand Down
12 changes: 7 additions & 5 deletions src/SoftRobots.Inverse/component/constraint/ForceSurfaceActuator.inl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -618,7 +618,8 @@ void ForceSurfaceActuator<DataTypes>::buildConstraintMatrix(const ConstraintPara

computeSurfaces();

m_constraintId = cIndex;
m_constraintIndex.setValue(cIndex);
const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex);

if(d_updateNormals.getValue() && m_useNormals)
computeNormals();
Expand All @@ -629,15 +630,15 @@ void ForceSurfaceActuator<DataTypes>::buildConstraintMatrix(const ConstraintPara
MatrixDeriv& matrix = *cMatrix.beginEdit();
for(unsigned int i=0; i<centers.size(); i++)
{
MatrixDerivRowIterator rowIterator = matrix.writeLine(m_constraintId+i);
MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex+i);
for(unsigned int j=0; j<m_pointsInSphereId[i].size(); j++)
rowIterator.addCol(m_pointsInSphereId[i][j], directions[i]*m_ratios[i][j]);

cIndex++;
}
cMatrix.endEdit();

m_nbLines = cIndex - m_constraintId;
m_nbLines = cIndex - constraintIndex;
}


Expand All @@ -649,8 +650,9 @@ void ForceSurfaceActuator<DataTypes>::getConstraintViolation(const ConstraintPar
SOFA_UNUSED(cParams);
SOFA_UNUSED(Jdx);

const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex);
for(unsigned int i=0; i<d_centers.getValue().size(); i++)
resV->set(m_constraintId+i, 0);
resV->set(constraintIndex+i, 0);
}


Expand Down
2 changes: 1 addition & 1 deletion src/SoftRobots.Inverse/component/constraint/JointActuator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ class JointActuator : public Actuator<DataTypes>
/// otherwise any access to the base::attribute would require
/// the "this->" approach.
using Actuator<DataTypes>::m_state ;
using Actuator<DataTypes>::m_constraintId ;
using Actuator<DataTypes>::m_constraintIndex ;
////////////////////////////////////////////////////////////////////////////


Expand Down
13 changes: 7 additions & 6 deletions src/SoftRobots.Inverse/component/constraint/JointActuator.inl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -223,17 +223,18 @@ void JointActuator<DataTypes>::buildConstraintMatrix(const ConstraintParams* cPa
if(!this->isComponentStateValid())
return ;

m_constraintId = cIndex;
m_constraintIndex.setValue(cIndex);
const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex);

MatrixDeriv& matrix = *cMatrix.beginEdit();

MatrixDerivRowIterator rowIterator = matrix.writeLine(m_constraintId);
MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex);
rowIterator.addCol(d_index.getValue(), Deriv(1.));
cIndex++;

cMatrix.endEdit();

m_nbLines = cIndex - m_constraintId;
m_nbLines = cIndex - constraintIndex;
}


Expand All @@ -248,7 +249,7 @@ void JointActuator<DataTypes>::getConstraintViolation(const ConstraintParams* cP
return ;

Real dFree = Jdx->element(0) - d_initAngle.getValue() + m_state->readPositions()[d_index.getValue()][0];
resV->set(m_constraintId, dFree);
resV->set(m_constraintIndex.getValue(), dFree);
}


Expand Down
2 changes: 1 addition & 1 deletion src/SoftRobots.Inverse/component/constraint/PositionEffector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ class PositionEffector : public Effector<DataTypes>, public softrobots::constrai
using softrobots::constraint::PositionModel<DataTypes>::d_indices ;
using softrobots::constraint::PositionModel<DataTypes>::d_directions ;
using softrobots::constraint::PositionModel<DataTypes>::d_useDirections ;
using softrobots::constraint::PositionModel<DataTypes>::m_constraintId ;
using softrobots::constraint::PositionModel<DataTypes>::m_constraintIndex ;
using softrobots::constraint::PositionModel<DataTypes>::d_weight ;
using softrobots::behavior::SoftRobotsConstraint<DataTypes>::m_state ;
using softrobots::behavior::SoftRobotsConstraint<DataTypes>::d_componentState ;
Expand Down
4 changes: 2 additions & 2 deletions src/SoftRobots.Inverse/component/constraint/PositionEffector.inl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,7 @@ void PositionEffector<DataTypes>::getConstraintViolation(const sofa::core::Const
const auto& weight = sofa::helper::getReadAccessor(d_weight);
const auto& indices = sofa::helper::getReadAccessor(d_indices);
sofa::Index sizeIndices = indices.size();

const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex);

int index = 0;
for (unsigned int i=0; i<sizeIndices; i++)
Expand All @@ -126,7 +126,7 @@ void PositionEffector<DataTypes>::getConstraintViolation(const sofa::core::Const
if(useDirections[j])
{
Real dfree = Jdx->element(index) + d*directions[j]*weight[j];
resV->set(m_constraintId+index, dfree);
resV->set(constraintIndex+index, dfree);
index++;
}
}
Expand Down
4 changes: 2 additions & 2 deletions src/SoftRobots.Inverse/component/constraint/PositionEquality.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,8 +105,8 @@ class PositionEquality : public Equality<DataTypes>, public PositionModel<DataTy

using Equality<DataTypes>::m_hasLambdaEqual;
using Equality<DataTypes>::m_lambdaEqual ;

using SoftRobotsBaseConstraint::m_constraintId ;
using SoftRobotsBaseConstraint::m_constraintIndex ;
using SoftRobotsBaseConstraint::m_nbLines ;
using Equality<DataTypes>::d_componentState ;

Expand Down
8 changes: 5 additions & 3 deletions src/SoftRobots.Inverse/component/constraint/PositionEquality.inl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,8 @@ void PositionEquality<DataTypes>::buildConstraintMatrix(const ConstraintParams*
const auto& indices = sofa::helper::getReadAccessor(d_indices);
const auto& directions = sofa::helper::getReadAccessor(d_directions);
const auto& useDirections = sofa::helper::getReadAccessor(d_useDirections);
m_constraintId = cIndex;
m_constraintIndex.setValue(cIndex);
const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex);

for (sofa::Index i: indices)
{
Expand All @@ -116,7 +117,7 @@ void PositionEquality<DataTypes>::buildConstraintMatrix(const ConstraintParams*
}

cMatrix.endEdit();
m_nbLines = cIndex - m_constraintId;
m_nbLines = cIndex - constraintIndex;
}


Expand All @@ -132,6 +133,7 @@ void PositionEquality<DataTypes>::getConstraintViolation(const ConstraintParams*
ReadAccessor<sofa::Data<VecCoord>> positions = m_state->readPositions();
const auto& indices = sofa::helper::getReadAccessor(d_indices);
const auto& useDirections = sofa::helper::getReadAccessor(d_useDirections);
const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex);

bool withJdx = (Jdx->size()!=0);

Expand All @@ -147,7 +149,7 @@ void PositionEquality<DataTypes>::getConstraintViolation(const ConstraintParams*
{
dfree += Jdx->element(index);
}
resV->set(m_constraintId + index++, dfree);
resV->set(constraintIndex + index++, dfree);
}
}
}
Expand Down
2 changes: 1 addition & 1 deletion src/SoftRobots.Inverse/component/constraint/SlidingActuator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,7 +118,7 @@ class SlidingActuator : public Actuator<DataTypes>
using Inherit::m_lambdaMin ;

using Inherit::m_nbLines ;
using Inherit::m_constraintId ;
using Inherit::m_constraintIndex ;
using Inherit::d_componentState ;

using Inherit::m_state ;
Expand Down
Loading

0 comments on commit c82eb54

Please sign in to comment.