Skip to content

Commit 9dc0589

Browse files
pariterrepariterre
authored
Merge pull request #204 from fbailly/master
Swig vector3d
2 parents aa5da40 + fdbcb12 commit 9dc0589

File tree

8 files changed

+118
-43
lines changed

8 files changed

+118
-43
lines changed

‎binding/python3/biorbd_python.i.in‎

Lines changed: 22 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010

1111
#include "BiorbdModel.h"
1212
#include "Utils/Matrix.h"
13+
#include "Utils/Vector3d.h"
1314
#include "RigidBody/GeneralizedCoordinates.h"
1415
#include "RigidBody/GeneralizedVelocity.h"
1516
#include "RigidBody/GeneralizedAcceleration.h"
@@ -810,15 +811,24 @@ biorbd::rigidbody::GeneralizedAcceleration * {
810811
}
811812
#endif
812813

813-
// --- Node --- //
814-
#ifndef BIORBD_USE_CASADI_MATH
815-
%typemap(typecheck, precedence=2130) biorbd::utils::Vector3d &{
814+
// --- Vector3d --- //
815+
%typemap(typecheck, precedence=2130)
816+
biorbd::utils::Vector3d &{
816817
void *argp1 = 0;
817818
if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__utils__Node, 0 | 0)) && argp1) {
818819
// Test if it is a pointer to SWIGTYPE_p_biorbd__utils__Node already exists
819820
$1 = true;
820-
} else if( PyArray_Check($input) ) {
821+
}
822+
#ifdef BIORBD_USE_CASADI_MATH
823+
else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) {
824+
// Test if it is a pointer to
825+
// SWIGTYPE_p_biorbd__utils__Vector3d already exists
826+
$1 = true;
827+
}
828+
#endif
829+
else if( PyArray_Check($input) ) {
821830
// test if it is a numpy array
831+
822832
$1 = true;
823833
} else {
824834
$1 = false;
@@ -829,7 +839,13 @@ biorbd::rigidbody::GeneralizedAcceleration * {
829839
if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_biorbd__utils__Node, 0 | 0)) && argp1) {
830840
// Recast the pointer
831841
$1 = reinterpret_cast< biorbd::utils::Vector3d * >(argp1);
832-
} else if( PyArray_Check($input) ) {
842+
}
843+
#ifdef BIORBD_USE_CASADI_MATH
844+
else if (SWIG_IsOK(SWIG_ConvertPtr($input, &argp1, SWIGTYPE_p_casadi__MX, 0 | 0)) && argp1) {
845+
$1 = new biorbd::utils::Vector3d(biorbd::utils::Vector(*reinterpret_cast<casadi::MX*>(argp1)));
846+
}
847+
#endif
848+
else if( PyArray_Check($input) ) {
833849
// Get dimensions of the data
834850
int ndim = PyArray_NDIM ((PyArrayObject*)$input);
835851
npy_intp* dims = PyArray_DIMS ((PyArrayObject*)$input);
@@ -852,7 +868,7 @@ biorbd::rigidbody::GeneralizedAcceleration * {
852868
SWIG_fail;
853869
}
854870
};
855-
#endif
871+
856872
%extend biorbd::utils::Vector3d{
857873
#ifdef BIORBD_USE_CASADI_MATH
858874
casadi::MX to_mx(){

‎examples/forwardDynamicsFromMusclesExample.cpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -25,9 +25,10 @@ int main()
2525
Qdot.setZero();
2626

2727
// Set all muscles to half of their maximal activation
28-
std::vector<std::shared_ptr<biorbd::muscles::State>> states;
28+
std::vector<std::shared_ptr<biorbd::muscles::State>> states = model.stateSet();
2929
for (unsigned int i=0; i<model.nbMuscles(); ++i){
30-
states.push_back(std::make_shared<biorbd::muscles::State>(0, 0.5));
30+
states[i]->setExcitation(0);
31+
states[i]->setActivation(0.5);
3132
}
3233

3334
// Proceed with the computation of joint torque from the muscles

‎include/Utils/Vector3d.h‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -118,7 +118,6 @@ class BIORBD_API Vector3d : public RigidBodyDynamics::Math::Vector3d, public bio
118118
///
119119
Vector3d(
120120
const RBDLCasadiMath::MX_Xd_SubMatrix& other);
121-
122121
#endif
123122

124123
///

‎src/Muscles/Geometry.cpp‎

Lines changed: 13 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
#include "Utils/Error.h"
77
#include "Utils/Matrix.h"
88
#include "Utils/RotoTrans.h"
9+
#include "RigidBody/NodeSegment.h"
910
#include "RigidBody/Joints.h"
1011
#include "RigidBody/GeneralizedCoordinates.h"
1112
#include "RigidBody/GeneralizedVelocity.h"
@@ -191,7 +192,12 @@ void biorbd::muscles::Geometry::updateKinematics(
191192
void biorbd::muscles::Geometry::setOrigin(
192193
const utils::Vector3d &position)
193194
{
194-
*m_origin = position;
195+
if (dynamic_cast<const biorbd::rigidbody::NodeSegment*>(&position)){
196+
*m_origin = position;
197+
} else {
198+
// Preserve the Node information
199+
m_origin->RigidBodyDynamics::Math::Vector3d::operator=(position);
200+
}
195201
}
196202
const biorbd::utils::Vector3d& biorbd::muscles::Geometry::originInLocal() const
197203
{
@@ -201,7 +207,12 @@ const biorbd::utils::Vector3d& biorbd::muscles::Geometry::originInLocal() const
201207
void biorbd::muscles::Geometry::setInsertionInLocal(
202208
const utils::Vector3d &position)
203209
{
204-
*m_insertion = position;
210+
if (dynamic_cast<const biorbd::rigidbody::NodeSegment*>(&position)){
211+
*m_insertion = position;
212+
} else {
213+
// Preserve the Node information
214+
m_insertion->RigidBodyDynamics::Math::Vector3d::operator=(position);
215+
}
205216
}
206217
const biorbd::utils::Vector3d &biorbd::muscles::Geometry::insertionInLocal() const
207218
{

‎src/Utils/RotoTrans.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -77,7 +77,7 @@ biorbd::utils::Vector3d biorbd::utils::RotoTrans::axe(unsigned int idx) const
7777
{
7878
biorbd::utils::Error::check(
7979
idx<=2, "Axis must be between 0 and 2 included");
80-
return static_cast<RigidBodyDynamics::Math::VectorNd>(rot().block(0,idx,3,1));
80+
return rot().block(0,idx,3,1);
8181
}
8282

8383
biorbd::utils::RotoTrans biorbd::utils::RotoTrans::transpose() const

‎src/Utils/Vector3d.cpp‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -50,7 +50,6 @@ biorbd::utils::Vector3d::Vector3d(
5050
setType();
5151
}
5252

53-
5453
biorbd::utils::Vector3d::Vector3d(
5554
const RigidBodyDynamics::Math::VectorNd &other) :
5655
RigidBodyDynamics::Math::Vector3d(other[0], other[1], other[2]),
@@ -61,7 +60,8 @@ biorbd::utils::Vector3d::Vector3d(
6160

6261
biorbd::utils::Vector3d::Vector3d(
6362
const RigidBodyDynamics::Math::Vector4d &other) :
64-
RigidBodyDynamics::Math::Vector3d(other[0], other[1], other[2]), biorbd::utils::Node ()
63+
RigidBodyDynamics::Math::Vector3d(other[0], other[1], other[2]),
64+
biorbd::utils::Node ()
6565
{
6666
setType();
6767
}
@@ -97,7 +97,7 @@ biorbd::utils::Vector3d biorbd::utils::Vector3d::applyRT(const biorbd::utils::Ro
9797
RigidBodyDynamics::Math::Vector4d v;
9898
v.block(0, 0, 3, 1) = *this;
9999
v[3] = 1;
100-
return static_cast<RigidBodyDynamics::Math::VectorNd>((rt * v).block(0, 0, 3, 1));
100+
return (rt * v).block(0, 0, 3, 1);
101101
}
102102

103103
void biorbd::utils::Vector3d::applyRT(

‎test/binding/Python3/test_conversion.py‎

Lines changed: 27 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -28,22 +28,42 @@ def test_np_mx_to_generalized():
2828

2929

3030
# --- Options --- #
31-
def imu_to_array():
31+
def test_imu_to_array():
3232
m = biorbd.Model("../../models/IMUandCustomRT/pyomecaman_withIMUs.bioMod")
33-
q = np.zeros( (m.nbQ(), ) )
33+
q = np.zeros((m.nbQ(),))
3434

3535
if biorbd.currentLinearAlgebraBackend() == 1:
3636
from casadi import MX
37+
3738
q_sym = MX.sym("q", m.nbQ(), 1)
3839
imu_func = biorbd.to_casadi_func("imu", m.IMU, q_sym)
3940
imu = imu_func(q)[:, :4]
4041

4142
else:
4243
imu = m.IMU(q)[0].to_array()
4344

44-
np.testing.assert_almost_equal(imu, np.array([
45-
[0.99003329, -0.09933467, 0.09983342, 0.26719],
46-
[0.10925158, 0.98903828, -0.09933467, 0.04783],
47-
[-0.08887169, 0.10925158, 0.99003329, -0.20946],
48-
[0., 0., 0., 1.]]))
45+
np.testing.assert_almost_equal(
46+
imu,
47+
np.array(
48+
[
49+
[0.99003329, -0.09933467, 0.09983342, 0.26719],
50+
[0.10925158, 0.98903828, -0.09933467, 0.04783],
51+
[-0.08887169, 0.10925158, 0.99003329, -0.20946],
52+
[0.0, 0.0, 0.0, 1.0],
53+
]
54+
),
55+
)
56+
57+
58+
def test_vector3d():
59+
biorbd_model = biorbd.Model()
60+
vec = np.random.rand(
61+
3,
62+
)
63+
biorbd_model.setGravity(vec)
64+
65+
if biorbd.currentLinearAlgebraBackend() == 1:
66+
from casadi import MX
4967

68+
vec = MX.ones(3, 1)
69+
biorbd_model.setGravity(vec)

‎test/test_muscles.cpp‎

Lines changed: 49 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
#include "RigidBody/GeneralizedVelocity.h"
1010
#include "RigidBody/GeneralizedAcceleration.h"
1111
#include "RigidBody/GeneralizedTorque.h"
12+
#include "RigidBody/NodeSegment.h"
1213
#ifdef MODULE_MUSCLES
1314
#include "Muscles/all.h"
1415
#include "Utils/String.h"
@@ -275,29 +276,56 @@ TEST(IdealizedActuator, copy)
275276
EXPECT_NEAR(deepCopyLaterLength, 0.066381977535807504, requiredPrecision);
276277
}
277278

278-
// Change the position of the insertion and pennation angle and compare again (length and insertion in Local)
279-
biorbd::muscles::Characteristics charac(idealizedActuator.characteristics());
280-
charac.setPennationAngle(0.523599);
281-
biorbd::utils::Vector3d insertion(idealizedActuator.position().insertionInLocal());
282-
insertion.set(0.2, 0.2, 0.2);
283-
biorbd::utils::String oldName(insertion.biorbd::utils::Node::name());
284-
biorbd::utils::String newName("MyNewName");
285-
insertion.setName(newName);
286-
idealizedActuator.updateOrientations(model, Q, qDot, 2);
279+
{
280+
// Change the position of the insertion and pennation angle and compare again (length and insertion in Local)
281+
biorbd::muscles::Characteristics charac(idealizedActuator.characteristics());
282+
charac.setPennationAngle(0.523599);
283+
biorbd::utils::Vector3d insertion(idealizedActuator.position().insertionInLocal());
284+
insertion.set(0.2, 0.2, 0.2);
285+
biorbd::utils::String oldName(insertion.biorbd::utils::Node::name());
286+
biorbd::utils::String newName("MyNewName");
287+
insertion.setName(newName);
288+
idealizedActuator.updateOrientations(model, Q, qDot, 2);
289+
290+
{
291+
SCALAR_TO_DOUBLE(length, idealizedActuator.position().length());
292+
SCALAR_TO_DOUBLE(shallowCopyLength, shallowCopy.position().length());
293+
SCALAR_TO_DOUBLE(deepCopyNowLength, deepCopyNow.position().length());
294+
SCALAR_TO_DOUBLE(deepCopyLaterLength, deepCopyLater.position().length());
295+
EXPECT_NEAR(length, 0.07570761027741163, requiredPrecision);
296+
EXPECT_NEAR(shallowCopyLength, 0.07570761027741163, requiredPrecision);
297+
EXPECT_NEAR(deepCopyNowLength, 0.066381977535807504, requiredPrecision);
298+
EXPECT_NEAR(deepCopyLaterLength, 0.066381977535807504, requiredPrecision);
299+
EXPECT_EQ(idealizedActuator.position().insertionInLocal().biorbd::utils::Node::name(), newName);
300+
EXPECT_EQ(shallowCopy.position().insertionInLocal().biorbd::utils::Node::name(), newName);
301+
EXPECT_EQ(deepCopyNow.position().insertionInLocal().biorbd::utils::Node::name(), oldName);
302+
EXPECT_EQ(deepCopyLater.position().insertionInLocal().biorbd::utils::Node::name(), oldName);
303+
}
304+
305+
}
287306

288307
{
289-
SCALAR_TO_DOUBLE(length, idealizedActuator.position().length());
290-
SCALAR_TO_DOUBLE(shallowCopyLength, shallowCopy.position().length());
291-
SCALAR_TO_DOUBLE(deepCopyNowLength, deepCopyNow.position().length());
292-
SCALAR_TO_DOUBLE(deepCopyLaterLength, deepCopyLater.position().length());
293-
EXPECT_NEAR(length, 0.07570761027741163, requiredPrecision);
294-
EXPECT_NEAR(shallowCopyLength, 0.07570761027741163, requiredPrecision);
295-
EXPECT_NEAR(deepCopyNowLength, 0.066381977535807504, requiredPrecision);
296-
EXPECT_NEAR(deepCopyLaterLength, 0.066381977535807504, requiredPrecision);
297-
EXPECT_EQ(idealizedActuator.position().insertionInLocal().biorbd::utils::Node::name(), newName);
298-
EXPECT_EQ(shallowCopy.position().insertionInLocal().biorbd::utils::Node::name(), newName);
299-
EXPECT_EQ(deepCopyNow.position().insertionInLocal().biorbd::utils::Node::name(), oldName);
300-
EXPECT_EQ(deepCopyLater.position().insertionInLocal().biorbd::utils::Node::name(), oldName);
308+
// Change the position giving an actual vector
309+
biorbd::utils::Vector3d newPosition(1, 2, 3);
310+
biorbd::utils::String oldName("MyNewName");
311+
biorbd::utils::String newName("MyNewNewName");
312+
biorbd::rigidbody::NodeSegment newNode(newPosition, newName, "", true, true, "", 0);
313+
{
314+
const_cast<biorbd::muscles::Geometry&>(idealizedActuator.position()).setOrigin(newPosition);
315+
const_cast<biorbd::muscles::Geometry&>(idealizedActuator.position()).setInsertionInLocal(newPosition);
316+
const biorbd::utils::Vector3d& origin = idealizedActuator.position().originInLocal();
317+
const biorbd::utils::Vector3d& insertion = idealizedActuator.position().insertionInLocal();
318+
EXPECT_STREQ(origin.biorbd::utils::Node::name().c_str(), "TRImed_origin");
319+
EXPECT_STREQ(insertion.biorbd::utils::Node::name().c_str(), oldName.c_str());
320+
}
321+
{
322+
const_cast<biorbd::muscles::Geometry&>(idealizedActuator.position()).setOrigin(newNode);
323+
const_cast<biorbd::muscles::Geometry&>(idealizedActuator.position()).setInsertionInLocal(newNode);
324+
const biorbd::utils::Vector3d& origin = idealizedActuator.position().originInLocal();
325+
const biorbd::utils::Vector3d& insertion = idealizedActuator.position().insertionInLocal();
326+
EXPECT_STREQ(origin.biorbd::utils::Node::name().c_str(), newName.c_str());
327+
EXPECT_STREQ(insertion.biorbd::utils::Node::name().c_str(), newName.c_str());
328+
}
301329
}
302330
}
303331

0 commit comments

Comments
 (0)