Skip to content

Commit

Permalink
Merge pull request #117 from roboticslab-uc3m/streaming-cartesian-con…
Browse files Browse the repository at this point in the history 
…trol

Implement streaming cartesian commands, create AmorCartesianControl
  • Loading branch information
@jgvictores
jgvictores authored Oct 27, 2017
2 parents 37571fa + 60cc2d2 commit 1236178
Show file tree
Hide file tree
Showing 47 changed files with 4,121 additions and 92 deletions.
17 changes: 12 additions & 5 deletions .travis.yml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,8 @@ before_install:
- '[[ "$TRAVIS_EVENT_TYPE" = cron ]] && export YARP_CHECKOUT=devel || export YARP_CHECKOUT=master'

install:
#-- Install cmake
- sudo apt-get install cmake
#-- Install eigen3
- sudo apt-get install libeigen3-dev
#-- Install cmake and eigen
- sudo apt-get install cmake libeigen3-dev
#-- Install yarp
- git clone --branch="$YARP_CHECKOUT" https://github.com/robotology/yarp
- cd yarp && mkdir build && cd build
Expand All @@ -42,6 +40,15 @@ install:
- make
- sudo make install
- cd ../..
#-- (For apps using proximity sensors) Install yarp-devices
- git clone https://github.com/roboticslab-uc3m/yarp-devices
- cd yarp-devices
- mkdir build && cd build
- cmake .. -DENABLE_OneCanBusOneWrapper=OFF -DENABLE_TwoCanBusThreeWrappers=OFF -DENABLE_dumpCanBus=OFF -DENABLE_checkCanBus=OFF -DENABLE_oneCanBusOneWrapper=OFF -DENABLE_launchManipulation=OFF -DENABLE_launchLocomotion=OFF -DENABLE_CuiAbsolute=OFF -DENABLE_CanBusHico=OFF -DENABLE_TechnosoftIpos=OFF -DENABLE_LacqueyFetch=OFF -DENABLE_TextilesHand=OFF -DENABLE_FakeJoint=OFF -DENABLE_CanBusControlboard=OFF -DENABLE_Jr3=OFF -DENABLE_SpaceNavigator=OFF -DENABLE_WiimoteSensor=OFF -DENABLE_LeapMotionSensor=OFF -DENABLE_tests=OFF
- make
- sudo make install
- export ROBOTICSLAB_YARP_DEVICES_DIR=$PWD
- cd ../..
# see http://gronlier.fr/blog/2015/01/adding-code-coverage-to-your-c-project/
- if [ "$CXX" = "g++" ]; then wget http://ftp.de.debian.org/debian/pool/main/l/lcov/lcov_1.11.orig.tar.gz; fi
- if [ "$CXX" = "g++" ]; then tar xf lcov_1.11.orig.tar.gz; fi
Expand All @@ -50,7 +57,7 @@ install:

before_script:
- mkdir -p build && cd build
- cmake .. -DENABLE_coverage=ON
- cmake .. -DENABLE_coverage=ON -DENABLE_AmorCartesianControl=OFF
- make
- sudo make install
- sudo ldconfig
Expand Down
214 changes: 204 additions & 10 deletions libraries/KinematicRepresentationLib/KinematicRepresentation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -213,25 +213,185 @@ bool KinRepresentation::decodePose(const std::vector<double> &x_in, std::vector<

// -----------------------------------------------------------------------------

bool KinRepresentation::encodeVelocity(const std::vector<double> &xdot_in, std::vector<double> &xdot_out,
coordinate_system coord, orientation_system orient, angular_units angle)
bool KinRepresentation::encodeVelocity(const std::vector<double> &x_in, const std::vector<double> &xdot_in,
std::vector<double> &xdot_out, coordinate_system coord, orientation_system orient, angular_units angle)
{
CD_ERROR("Not implemented.\n");
return false;
int expectedSize;

if (!checkVectorSize(xdot_in, orient, &expectedSize))
{
CD_ERROR("Size error; expected: %d, was: %d\n", expectedSize, xdot_in.size());
return false;
}

// expand current size if needed, but never truncate
xdot_out.resize(std::max<int>(6, xdot_out.size()));

switch (orient)
{
case AXIS_ANGLE:
{
KDL::Rotation rot = KDL::Rotation::Rot(KDL::Vector(xdot_in[3], xdot_in[4], xdot_in[5]), degToRadHelper(angle, xdot_in[6]));
KDL::Vector axis = rot.GetRot();
xdot_out[3] = axis.x();
xdot_out[4] = axis.y();
xdot_out[5] = axis.z();
break;
}
case AXIS_ANGLE_SCALED:
{
xdot_out[3] = degToRadHelper(angle, xdot_in[3]);
xdot_out[4] = degToRadHelper(angle, xdot_in[4]);
xdot_out[5] = degToRadHelper(angle, xdot_in[5]);
break;
}
case RPY:
{
// [0 -sa ca*cb]
// [0 ca sa*cb]
// [1 0 -sb]
// where a (alpha): z, b (beta): y, g (gamma, unused): x
// FIXME: really? review this, check which angle corresponds to which coordinate
KDL::Vector colX = KDL::Rotation::Identity().UnitZ();
KDL::Rotation rotZ = KDL::Rotation::RotZ(degToRadHelper(angle, x_in[5]));
KDL::Vector colY = rotZ * KDL::Rotation::Identity().UnitY();
KDL::Vector colZ = rotZ * KDL::Rotation::RotY(degToRadHelper(angle, x_in[4])) * KDL::Rotation::Identity().UnitX();
KDL::Rotation rot(colX, colY, colZ);
KDL::Vector v_in(degToRadHelper(angle, xdot_in[3]), degToRadHelper(angle, xdot_in[4]), degToRadHelper(angle, xdot_in[5]));
KDL::Vector v_out = rot * v_in;
xdot_out[3] = v_out.x();
xdot_out[4] = v_out.y();
xdot_out[5] = v_out.z();
break;
}
case EULER_YZ:
{
break;
}
case EULER_ZYZ:
{
break;
}
default:
return false;
}

// truncate extra elements
xdot_out.resize(6);

switch (coord)
{
case CARTESIAN:
xdot_out[0] = xdot_in[0];
xdot_out[1] = xdot_in[1];
xdot_out[2] = xdot_in[2];
break;
case CYLINDRICAL:
CD_ERROR("Not implemented.\n");
return false;
case SPHERICAL:
CD_ERROR("Not implemented.\n");
return false;
default:
return false;
}

return true;
}

// -----------------------------------------------------------------------------

bool KinRepresentation::decodeVelocity(const std::vector<double> &xdot_in, std::vector<double> &xdot_out,
coordinate_system coord, orientation_system orient, angular_units angle)
bool KinRepresentation::decodeVelocity(const std::vector<double> &x_in, const std::vector<double> &xdot_in,
std::vector<double> &xdot_out, coordinate_system coord, orientation_system orient, angular_units angle)
{
CD_ERROR("Not implemented.\n");
return false;
int expectedSize;

if (!checkVectorSize(xdot_in, AXIS_ANGLE_SCALED, &expectedSize))
{
CD_ERROR("Size error; expected: %d, was: %d\n", expectedSize, xdot_in.size());
return false;
}

switch (orient)
{
case AXIS_ANGLE:
{
xdot_out.resize(std::max<int>(7, xdot_out.size()));
KDL::Vector axis(xdot_in[3], xdot_in[4], xdot_in[5]);
xdot_out[6] = radToDegHelper(angle, axis.Norm());
axis.Normalize();
xdot_out[3] = axis.x();
xdot_out[4] = axis.y();
xdot_out[5] = axis.z();
xdot_out.resize(7);
break;
}
case AXIS_ANGLE_SCALED:
{
xdot_out.resize(std::max<int>(6, xdot_out.size()));
xdot_out[3] = radToDegHelper(angle, xdot_in[3]);
xdot_out[4] = radToDegHelper(angle, xdot_in[4]);
xdot_out[5] = radToDegHelper(angle, xdot_in[5]);
xdot_out.resize(6);
break;
}
case RPY:
{
// FIXME: see note at 'encodeVelocity'
xdot_out.resize(std::max<int>(6, xdot_out.size()));
KDL::Vector colX = KDL::Rotation::Identity().UnitZ();
KDL::Rotation rotZ = KDL::Rotation::RotZ(x_in[5]);
KDL::Vector colY = rotZ * KDL::Rotation::Identity().UnitY();
KDL::Vector colZ = rotZ * KDL::Rotation::RotY(x_in[4]) * KDL::Rotation::Identity().UnitX();
KDL::Rotation rot(colX, colY, colZ);
KDL::Vector v_in(xdot_in[3], xdot_in[4], xdot_in[5]);
KDL::Vector v_out = rot.Inverse() * v_in;
xdot_out[3] = radToDegHelper(angle, v_out.x());
xdot_out[4] = radToDegHelper(angle, v_out.y());
xdot_out[5] = radToDegHelper(angle, v_out.z());
xdot_out.resize(6);
break;
}
case EULER_YZ:
{
xdot_out.resize(std::max<int>(5, xdot_out.size()));
xdot_out.resize(5);
break;
}
case EULER_ZYZ:
{
xdot_out.resize(std::max<int>(6, xdot_out.size()));
xdot_out.resize(6);
break;
}
default:
return false;
}

switch (coord)
{
case CARTESIAN:
xdot_out[0] = xdot_in[0];
xdot_out[1] = xdot_in[1];
xdot_out[2] = xdot_in[2];
break;
case CYLINDRICAL:
CD_ERROR("Not implemented.\n");
return false;
case SPHERICAL:
CD_ERROR("Not implemented.\n");
return false;
default:
return false;
}

return true;
}

// -----------------------------------------------------------------------------

bool KinRepresentation::encodeAcceleration(const std::vector<double> &xdotdot_in, std::vector<double> &xdotdot_out,
bool KinRepresentation::encodeAcceleration(const std::vector<double> &x_in, const std::vector<double> &xdot_in,
const std::vector<double> &xdotdot_in, std::vector<double> &xdotdot_out,
coordinate_system coord, orientation_system orient, angular_units angle)
{
CD_ERROR("Not implemented.\n");
Expand All @@ -240,7 +400,8 @@ bool KinRepresentation::encodeAcceleration(const std::vector<double> &xdotdot_in

// -----------------------------------------------------------------------------

bool KinRepresentation::decodeAcceleration(const std::vector<double> &xdotdot_in, std::vector<double> &xdotdot_out,
bool KinRepresentation::decodeAcceleration(const std::vector<double> &x_in, const std::vector<double> &xdot_in,
const std::vector<double> &xdotdot_in, std::vector<double> &xdotdot_out,
coordinate_system coord, orientation_system orient, angular_units angle)
{
CD_ERROR("Not implemented.\n");
Expand All @@ -249,6 +410,39 @@ bool KinRepresentation::decodeAcceleration(const std::vector<double> &xdotdot_in

// -----------------------------------------------------------------------------

bool KinRepresentation::parseEnumerator(const std::string &str, orientation_system *orient, orientation_system fallback)
{
if (str == "axisAngle")
{
*orient = AXIS_ANGLE;
}
else if (str == "axisAngleScaled")
{
*orient = AXIS_ANGLE_SCALED;
}
else if (str == "RPY")
{
*orient = RPY;
}
else if (str == "eulerYZ")
{
*orient = EULER_YZ;
}
else if (str == "eulerZYZ")
{
*orient = EULER_ZYZ;
}
else
{
*orient = fallback;
return false;
}

return true;
}

// -----------------------------------------------------------------------------

bool KinRepresentation::checkVectorSize(const std::vector<double> &v_in, orientation_system orient, int *expectedSize)
{
switch (orient)
Expand Down
Loading

0 comments on commit 1236178

Please sign in to comment.