Skip to content

Commit

Permalink
IJointCoupling: final draft
Browse files Browse the repository at this point in the history
  • Loading branch information
@Nicogene
Nicogene committed Nov 14, 2023
1 parent c09d231 commit 5d9e3c3
Show file tree
Hide file tree
Showing 6 changed files with 162 additions and 101 deletions.
90 changes: 61 additions & 29 deletions src/devices/fakeJointCoupling/fakeJointCoupling.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,11 @@ YARP_LOG_COMPONENT(FAKEJOINTCOUPLING, "yarp.device.fakeJointCoupling")
}

bool FakeJointCoupling::open(yarp::os::Searchable &par) {
yarp::sig::VectorOf<size_t> coupled_physical_joints {3,4};
yarp::sig::VectorOf<size_t> coupled_physical_joints {2,3};
yarp::sig::VectorOf<size_t> coupled_actuated_axes {2};
std::vector<std::string> physical_joint_names{"phys_joint_0", "phys_joint_1", "phys_joint_2", "phys_joint_3", "phys_joint_4"};
std::vector<std::string> physical_joint_names{"phys_joint_0", "phys_joint_1", "phys_joint_2", "phys_joint_3"};
std::vector<std::string> actuated_axes_names{"act_axes_0", "act_axes_1", "act_axes_2"};
std::vector<std::pair<double, double>> physical_joint_limits{{-30.0, 30.0}, {-10.0, 10.0}, {-32.0, 33.0}, {0.0, 120.0}, {-20.0, 180.0}};
std::vector<std::pair<double, double>> physical_joint_limits{{-30.0, 30.0}, {-10.0, 10.0}, {-32.0, 33.0}, {0.0, 120.0}};
initialise(coupled_physical_joints, coupled_actuated_axes, physical_joint_names, actuated_axes_names, physical_joint_limits);
return true;
}
Expand All @@ -41,65 +41,97 @@ bool FakeJointCoupling::close() {
}

bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) {
if (physJointsPos.size() != actAxesPos.size()) {
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input and output vectors have different size";
size_t nrOfPhysicalJoints;
size_t nrOfActuatedAxes;
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
if (!ok || physJointsPos.size() != nrOfPhysicalJoints || actAxesPos.size() != nrOfActuatedAxes) {
// yCDebug(FAKEJOINTCOUPLING) << ok <<physJointsPos.size()<<nrOfPhysicalJoints<<actAxesPos.size()<<nrOfActuatedAxes;
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
return false;
}
std::transform(physJointsPos.begin(), physJointsPos.end(), actAxesPos.begin(), [](double pos) { return pos * 2; });
actAxesPos[0] = physJointsPos[0];
actAxesPos[1] = physJointsPos[1];
actAxesPos[2] = physJointsPos[2] + physJointsPos[3];
return true;
}
bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) {
if(physJointsPos.size() != physJointsVel.size() || physJointsPos.size() != actAxesVel.size()) {
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesVel: input and output vectors have different size";
size_t nrOfPhysicalJoints;
size_t nrOfActuatedAxes;
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
if (!ok || physJointsPos.size() != nrOfPhysicalJoints || physJointsVel.size() != nrOfPhysicalJoints || actAxesVel.size() != nrOfActuatedAxes) {
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
return false;
}
for(size_t i = 0; i < physJointsPos.size(); i++) {
actAxesVel[i] = 2 * physJointsPos[i] * + 2 * physJointsVel[i];
}
actAxesVel[0] = physJointsVel[0];
actAxesVel[1] = physJointsVel[1];
actAxesVel[2] = physJointsPos[2] + physJointsPos[3] + physJointsVel[2] + physJointsVel[3];
return true;
}
bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel,
const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) {
if(physJointsPos.size() != physJointsVel.size() || physJointsPos.size() != physJointsAcc.size() || physJointsPos.size() != actAxesAcc.size()) {
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesAcc: input and output vectors have different size";
size_t nrOfPhysicalJoints;
size_t nrOfActuatedAxes;
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
if(!ok || physJointsPos.size() != nrOfPhysicalJoints || physJointsVel.size() != nrOfPhysicalJoints || physJointsAcc.size() != nrOfPhysicalJoints || actAxesAcc.size() != nrOfActuatedAxes) {
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
return false;
}
for(size_t i = 0; i < physJointsPos.size(); i++) {
actAxesAcc[i] = 2 * physJointsPos[i] + 2 * physJointsVel[i] + 2* physJointsAcc[i];
}
actAxesAcc[0] = physJointsAcc[0];
actAxesAcc[1] = physJointsAcc[1];
actAxesAcc[2] = physJointsPos[2] + physJointsPos[3] + physJointsVel[2] + physJointsVel[3] + physJointsAcc[2] + physJointsAcc[3];
return true;
}
bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) {
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesTrq: not implemented yet";
return false;
}
bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) {
if(actAxesPos.size() != physJointsPos.size()) {
yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsPos: input and output vectors have different size";
size_t nrOfPhysicalJoints;
size_t nrOfActuatedAxes;
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
if(!ok || actAxesPos.size() != nrOfActuatedAxes || physJointsPos.size() != nrOfPhysicalJoints) {
yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsPos: input or output vectors have wrong size";
return false;
}
std::transform(actAxesPos.begin(), actAxesPos.end(), physJointsPos.begin(), [](double pos) { return pos / 2.0; });
physJointsPos[0] = actAxesPos[0];
physJointsPos[1] = actAxesPos[1];
physJointsPos[2] = actAxesPos[2] / 2.0;
physJointsPos[3] = actAxesPos[2] / 2.0;
return true;
}
bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) {
if(actAxesPos.size() != actAxesVel.size() || actAxesPos.size() != physJointsVel.size()) {
yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsVel: input and output vectors have different size";
size_t nrOfPhysicalJoints;
size_t nrOfActuatedAxes;
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
if(!ok || actAxesPos.size() != nrOfActuatedAxes || actAxesVel.size() != nrOfActuatedAxes || physJointsVel.size() != nrOfPhysicalJoints) {
yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsVel: input or output vectors have wrong size";
return false;
}
for(size_t i = 0; i < actAxesPos.size(); i++) {
physJointsVel[i] = actAxesPos[i] / 2.0 + actAxesVel[i] / 2.0;
}
physJointsVel[0] = actAxesVel[0];
physJointsVel[1] = actAxesVel[1];
physJointsVel[2] = actAxesPos[2] / 2.0 - actAxesVel[2] / 2.0;
physJointsVel[3] = actAxesPos[2] / 2.0 + actAxesVel[2] / 2.0;
return true;

}
bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) {
if(actAxesPos.size() != actAxesVel.size() || actAxesPos.size() != actAxesAcc.size() || actAxesPos.size() != physJointsAcc.size()) {
yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsAcc: input and output vectors have different size";
size_t nrOfPhysicalJoints;
size_t nrOfActuatedAxes;
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
if(!ok || actAxesPos.size() != nrOfActuatedAxes || actAxesVel.size() != nrOfActuatedAxes || actAxesAcc.size() != nrOfActuatedAxes || physJointsAcc.size() != nrOfPhysicalJoints) {
yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsAcc: input or output vectors have wrong size";
return false;
}
for(size_t i = 0; i < actAxesPos.size(); i++) {
physJointsAcc[i] = actAxesPos[i] / 2.0 + actAxesVel[i] / 2.0 + actAxesAcc[i] / 2.0;
}
physJointsAcc[0] = actAxesAcc[0];
physJointsAcc[1] = actAxesAcc[1];
physJointsAcc[2] = actAxesPos[2] / 2.0 - actAxesVel[2] / 2.0 - actAxesAcc[2] / 2.0;
physJointsAcc[3] = actAxesPos[2] / 2.0 + actAxesVel[2] / 2.0 + actAxesAcc[2] / 2.0;
return true;
}
bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) {
Expand Down
2 changes: 1 addition & 1 deletion src/devices/fakeJointCoupling/fakeJointCoupling.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
/**
* @ingroup dev_impl_fake dev_impl_motor
*
* \brief `fakeMotionControl`: Documentation to be added
* \brief `fakeJointCoupling`: Documentation to be added
*
* The aim of this device is to mimic the expected behavior of a
* joint coupling device to help testing the high level software.
Expand Down
95 changes: 73 additions & 22 deletions src/devices/fakeJointCoupling/tests/fakeJointCoupling_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,15 +30,13 @@ TEST_CASE("dev::fakeJointCoupling", "[yarp::dev]")
}

REQUIRE(ddjc.view(ijc));



REQUIRE(ijc!=nullptr);
yarp::sig::VectorOf<size_t> coupled_physical_joints;
coupled_physical_joints.clear();
CHECK(ijc->getCoupledPhysicalJoints(coupled_physical_joints));
CHECK(coupled_physical_joints.size() == 2);
CHECK(coupled_physical_joints[0] == 3);
CHECK(coupled_physical_joints[1] == 4);
CHECK(coupled_physical_joints[0] == 2);
CHECK(coupled_physical_joints[1] == 3);

yarp::sig::VectorOf<size_t> coupled_actuated_axes;
coupled_actuated_axes.clear();
Expand All @@ -47,11 +45,11 @@ TEST_CASE("dev::fakeJointCoupling", "[yarp::dev]")
CHECK(coupled_actuated_axes[0] == 2);

size_t nr_of_physical_joints{0};
CHECK(ijc->getNrOfPhysicalJoints(&nr_of_physical_joints));
CHECK(nr_of_physical_joints == 5);
CHECK(ijc->getNrOfPhysicalJoints(nr_of_physical_joints));
CHECK(nr_of_physical_joints == 4);

size_t nr_of_actuated_axes{0};
CHECK(ijc->getNrOfActuatedAxes(&nr_of_actuated_axes));
CHECK(ijc->getNrOfActuatedAxes(nr_of_actuated_axes));
CHECK(nr_of_actuated_axes == 3);

std::string physical_joint_name;
Expand All @@ -61,31 +59,84 @@ TEST_CASE("dev::fakeJointCoupling", "[yarp::dev]")
CHECK(physical_joint_name == "phys_joint_1");
CHECK(ijc->getPhysicalJointName(2, physical_joint_name));
CHECK(physical_joint_name == "phys_joint_2");
CHECK(ijc->getPhysicalJointName(3, physical_joint_name));
CHECK(physical_joint_name == "phys_joint_3");
CHECK_FALSE(ijc->getPhysicalJointName(4, physical_joint_name));

std::string actuated_axis_name;
CHECK(ijc->getActuatedAxisName(0, actuated_axis_name));
CHECK(actuated_axis_name == "act_axes_0");
CHECK(ijc->getActuatedAxisName(1, actuated_axis_name));
CHECK(actuated_axis_name == "act_axes_1");
CHECK(ijc->getActuatedAxisName(2, actuated_axis_name));
CHECK(actuated_axis_name == "act_axes_2");
CHECK_FALSE(ijc->getActuatedAxisName(3, actuated_axis_name));

yarp::sig::Vector physJointsPos{0.0, 1.0, 2.0, 3.0};
yarp::sig::Vector actAxesPos{0.0, 0.0, 0.0};
yarp::sig::Vector physJointsVel{0.0, 1.0, 2.0, 3.0};
yarp::sig::Vector actAxesVel{0.0, 0.0, 0.0};
yarp::sig::Vector physJointsAcc{0.0, 1.0, 2.0, 3.0};
yarp::sig::Vector actAxesAcc{0.0, 0.0, 0.0};;

CHECK(ijc->convertFromPhysicalJointsToActuatedAxesPos(physJointsPos, actAxesPos));
CHECK(actAxesPos[0] == 0.0);
CHECK(actAxesPos[1] == 1.0);
CHECK(actAxesPos[2] == 5.0);

CHECK(ijc->convertFromPhysicalJointsToActuatedAxesVel(physJointsPos, physJointsVel, actAxesVel));
CHECK(actAxesVel[0] == 0.0);
CHECK(actAxesVel[1] == 1.0);
CHECK(actAxesVel[2] == 10.0);

CHECK(ijc->convertFromPhysicalJointsToActuatedAxesAcc(physJointsPos, physJointsVel, physJointsAcc, actAxesAcc));
CHECK(actAxesAcc[0] == 0.0);
CHECK(actAxesAcc[1] == 1.0);
CHECK(actAxesAcc[2] == 15.0);

CHECK_FALSE(ijc->convertFromPhysicalJointsToActuatedAxesTrq(physJointsPos, physJointsAcc, actAxesAcc));

CHECK(ijc->convertFromActuatedAxesToPhysicalJointsPos(actAxesPos, physJointsPos));
CHECK(physJointsPos[0] == 0.0);
CHECK(physJointsPos[1] == 1.0);
CHECK(physJointsPos[2] == 2.5);
CHECK(physJointsPos[3] == 2.5);

CHECK(ijc->convertFromActuatedAxesToPhysicalJointsVel(actAxesPos, actAxesVel, physJointsVel));
CHECK(physJointsVel[0] == 0.0);
CHECK(physJointsVel[1] == 1.0);
CHECK(physJointsVel[2] == -2.5);
CHECK(physJointsVel[3] == 7.5);

CHECK(ijc->convertFromActuatedAxesToPhysicalJointsAcc(actAxesPos, actAxesVel, actAxesAcc, physJointsAcc));
CHECK(physJointsAcc[0] == 0.0);
CHECK(physJointsAcc[1] == 1.0);
CHECK(physJointsAcc[2] == -10.0);
CHECK(physJointsAcc[3] == 15.0);

CHECK_FALSE(ijc->convertFromActuatedAxesToPhysicalJointsTrq(actAxesPos, actAxesAcc, physJointsAcc));

double min{0.0}, max{0.0};
CHECK(ijc->getPhysicalJointLimits(0, min, max));
CHECK(min == -30.0);
CHECK(max == 30.0);
CHECK(ijc->getPhysicalJointLimits(1, min, max));
CHECK(min == -10.0);
CHECK(max == 10.0);
CHECK(ijc->getPhysicalJointLimits(2, min, max));
CHECK(min == -32.0);
CHECK(max == 33.0);
CHECK(ijc->getPhysicalJointLimits(3, min, max));
CHECK(min == 0.0);
CHECK(max == 120.0);




//"Close all polydrivers and check"
{
CHECK(ddjc.close());
}




// virtual bool convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) = 0;
// virtual bool convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) = 0;
// virtual bool convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) = 0;
// virtual bool convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) = 0;
// virtual bool convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) = 0;
// virtual bool convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) = 0;
// virtual bool convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) = 0;
// virtual bool convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) = 0;
// virtual bool getCoupledActuatedAxes(yarp::sig::VectorOf<size_t>& coupActAxesIndexes)=0;
// virtual bool getPhysicalJointLimits(size_t physicalJointIndex, double& min, double& max)=0;
}

Network::setLocalMode(false);
Expand Down
4 changes: 2 additions & 2 deletions src/libYARP_dev/src/yarp/dev/IJointCoupling.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -179,15 +179,15 @@ class YARP_dev_API yarp::dev::IJointCoupling
* @param[out] nrOfPhysicalJoints The number of physical joints
* @return true/false on success/failure
*/
virtual bool getNrOfPhysicalJoints(size_t* nrOfPhysicalJoints) = 0;
virtual bool getNrOfPhysicalJoints(size_t& nrOfPhysicalJoints) = 0;

/**
* @brief Get the number of actuated axes
*
* @param nrOfActuatedAxes The number of actuated axes
* @return true/false on success/failure
*/
virtual bool getNrOfActuatedAxes(size_t* nrOfActuatedAxes) = 0;
virtual bool getNrOfActuatedAxes(size_t& nrOfActuatedAxes) = 0;

/**
* @brief Return the vector of "physical joints indices" (i.e. numbers from 0 to n-1)
Expand Down
Loading

0 comments on commit 5d9e3c3

Please sign in to comment.