Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[constraint] Access constraints' index #30

Merged
merged 2 commits into from
Jun 4, 2024
Merged

[constraint] Access constraints' index #30

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f2ab448
[all] allows users to access the constraint's index in the constraint…
EulalieCoevoet Apr 29, 2024
9db2b14
Merge branch 'main' into pr_accessconstraintindex
EulalieCoevoet May 16, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions src/SoftRobots.Inverse/binding/Binding_QPInverseProblemSolver_doc.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading