Skip to content

Commit

Permalink
Merge branch 'fix-121-controller-conf' into develop
Browse files Browse the repository at this point in the history
  • Loading branch information
@PeterBowman
PeterBowman committed Jan 20, 2018
2 parents b0ccbf2 + aeae9df commit 944ec2a
Show file tree
Hide file tree
Showing 14 changed files with 212 additions and 129 deletions.
33 changes: 33 additions & 0 deletions libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,8 @@

#include <ColorDebug.hpp>

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

bool roboticslab::AmorCartesianControl::waitForCompletion(int vocab)
{
currentState = vocab;
Expand Down Expand Up @@ -51,3 +53,34 @@ bool roboticslab::AmorCartesianControl::checkJointVelocities(const std::vector<d
}

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

bool roboticslab::AmorCartesianControl::performDiffInvKin(const std::vector<double> & currentQ,
const std::vector<double> & xdot,
std::vector<double> & qdot)
{
if (referenceFrame == BASE_FRAME)
{
if (!iCartesianSolver->diffInvKin(currentQ, xdot, qdot))
{
CD_ERROR("diffInvKin failed.\n");
return false;
}
}
else if (referenceFrame == TCP_FRAME)
{
if (!iCartesianSolver->diffInvKinEE(currentQ, xdot, qdot))
{
CD_ERROR("diffInvKinEE failed.\n");
return false;
}
}
else
{
CD_ERROR("Unsupported reference frame.\n");
return false;
}

return true;
}

// -----------------------------------------------------------------------------
6 changes: 5 additions & 1 deletion libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,9 +107,13 @@ class AmorCartesianControl : public yarp::dev::DeviceDriver, public ICartesianCo

bool checkJointVelocities(const std::vector<double> &qdot);

bool performDiffInvKin(const std::vector<double> & currentQ,
const std::vector<double> & xdot,
std::vector<double> & qdot);

AMOR_HANDLE handle;
bool ownsHandle;

yarp::dev::PolyDriver cartesianDevice;
roboticslab::ICartesianSolver *iCartesianSolver;

Expand Down
81 changes: 39 additions & 42 deletions libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,6 @@

#include "AmorCartesianControl.hpp"

#include <cmath>

#include <yarp/os/Vocab.h>

#include <ColorDebug.hpp>
Expand Down Expand Up @@ -43,6 +41,12 @@ bool roboticslab::AmorCartesianControl::stat(int &state, std::vector<double> &x)

bool roboticslab::AmorCartesianControl::inv(const std::vector<double> &xd, std::vector<double> &q)
{
if (referenceFrame == TCP_FRAME)
{
CD_WARNING("TCP frame not supported yet in inv command.\n");
return false;
}

AMOR_VECTOR7 positions;

if (amor_get_actual_positions(handle, &positions) != AMOR_SUCCESS)
Expand Down Expand Up @@ -71,6 +75,12 @@ bool roboticslab::AmorCartesianControl::inv(const std::vector<double> &xd, std::

bool roboticslab::AmorCartesianControl::movj(const std::vector<double> &xd)
{
if (referenceFrame == TCP_FRAME)
{
CD_WARNING("TCP frame not supported yet in movj command.\n");
return false;
}

std::vector<double> qd;

if (!inv(xd, qd))
Expand Down Expand Up @@ -99,6 +109,12 @@ bool roboticslab::AmorCartesianControl::movj(const std::vector<double> &xd)

bool roboticslab::AmorCartesianControl::relj(const std::vector<double> &xd)
{
if (referenceFrame == TCP_FRAME)
{
CD_WARNING("TCP frame not supported yet in relj command.\n");
return false;
}

int state;
std::vector<double> x;

Expand All @@ -120,6 +136,12 @@ bool roboticslab::AmorCartesianControl::relj(const std::vector<double> &xd)

bool roboticslab::AmorCartesianControl::movl(const std::vector<double> &xd)
{
if (referenceFrame == TCP_FRAME)
{
CD_WARNING("TCP frame not supported yet in movl command.\n");
return false;
}

std::vector<double> xd_rpy;

KinRepresentation::decodePose(xd, xd_rpy, KinRepresentation::CARTESIAN, KinRepresentation::RPY);
Expand Down Expand Up @@ -147,6 +169,12 @@ bool roboticslab::AmorCartesianControl::movl(const std::vector<double> &xd)

bool roboticslab::AmorCartesianControl::movv(const std::vector<double> &xdotd)
{
if (referenceFrame == TCP_FRAME)
{
CD_WARNING("TCP frame not supported yet in movv command.\n");
return false;
}

int state;
std::vector<double> xCurrent;

Expand Down Expand Up @@ -240,21 +268,10 @@ void roboticslab::AmorCartesianControl::twist(const std::vector<double> &xdot)
currentQ[i] = KinRepresentation::radToDeg(positions[i]);
}

if (referenceFrame == BASE_FRAME)
{
if (!iCartesianSolver->diffInvKin(currentQ, xdot, qdot))
{
CD_ERROR("diffInvKin failed.\n");
return;
}
}
else if (referenceFrame == TCP_FRAME)
if (!performDiffInvKin(currentQ, xdot, qdot))
{
if (!iCartesianSolver->diffInvKinEE(currentQ, xdot, qdot))
{
CD_ERROR("diffInvKinEE failed.\n");
return;
}
CD_ERROR("Cannot perform differential IK.\n");
return;
}

if (!checkJointVelocities(qdot))
Expand Down Expand Up @@ -314,21 +331,10 @@ void roboticslab::AmorCartesianControl::pose(const std::vector<double> &x, doubl

std::vector<double> qdot;

if (referenceFrame == BASE_FRAME)
if (!performDiffInvKin(currentQ, xdot, qdot))
{
if (!iCartesianSolver->diffInvKin(currentQ, xdot, qdot))
{
CD_ERROR("diffInvKin failed.\n");
return;
}
}
else if (referenceFrame == TCP_FRAME)
{
if (!iCartesianSolver->diffInvKinEE(currentQ, xdot, qdot))
{
CD_ERROR("diffInvKinEE failed.\n");
return;
}
CD_ERROR("Cannot perform differential IK.\n");
return;
}

if (!checkJointVelocities(qdot))
Expand Down Expand Up @@ -363,7 +369,6 @@ bool roboticslab::AmorCartesianControl::setParameter(int vocab, double value)
CD_ERROR("Controller gain cannot be negative.\n");
return false;
}

gain = value;
break;
case VOCAB_CC_CONFIG_MAX_JOINT_VEL:
Expand All @@ -372,23 +377,15 @@ bool roboticslab::AmorCartesianControl::setParameter(int vocab, double value)
CD_ERROR("Maximum joint velocity cannot be negative nor zero.\n");
return false;
}

maxJointVelocity = value;
break;
case VOCAB_CC_CONFIG_FRAME:
switch ((int)value)
if (value != BASE_FRAME && value != TCP_FRAME)
{
case VOCAB_CC_CONFIG_FRAME_BASE:
referenceFrame = BASE_FRAME;
break;
case VOCAB_CC_CONFIG_FRAME_TCP:
referenceFrame = TCP_FRAME;
break;
default:
CD_ERROR("Unrecognized of unsupported reference frame vocab: %s.\n", yarp::os::Vocab::decode((int)value).c_str());
CD_ERROR("Unrecognized of unsupported reference frame vocab.\n");
return false;
}

referenceFrame = static_cast<reference_frame>(value);
break;
default:
CD_ERROR("Unrecognized or unsupported config parameter key: %s.\n", yarp::os::Vocab::decode(vocab).c_str());
Expand Down
38 changes: 38 additions & 0 deletions libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-

#include "BasicCartesianControl.hpp"

#include <ColorDebug.hpp>

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

bool roboticslab::BasicCartesianControl::performDiffInvKin(const std::vector<double> & currentQ,
const std::vector<double> & xdot,
std::vector<double> & qdot)
{
if (referenceFrame == BASE_FRAME)
{
if (!iCartesianSolver->diffInvKin(currentQ, xdot, qdot))
{
CD_ERROR("diffInvKin failed.\n");
return false;
}
}
else if (referenceFrame == TCP_FRAME)
{
if (!iCartesianSolver->diffInvKinEE(currentQ, xdot, qdot))
{
CD_ERROR("diffInvKinEE failed.\n");
return false;
}
}
else
{
CD_ERROR("Unsupported reference frame.\n");
return false;
}

return true;
}

// -----------------------------------------------------------------------------
11 changes: 8 additions & 3 deletions libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,11 +19,11 @@
#define DEFAULT_SOLVER "KdlSolver"
#define DEFAULT_ROBOT "remote_controlboard"
#define DEFAULT_INIT_STATE VOCAB_CC_NOT_CONTROLLING
#define DEFAULT_MS 50
#define DEFAULT_GAIN 0.05
#define DEFAULT_QDOT_LIMIT 10.0
#define DEFAULT_REFERENCE_FRAME "base"
#define DEFAULT_DURATION 10.0
#define DEFAULT_CMC_RATE_MS 50
#define DEFAULT_REFERENCE_FRAME "base"

namespace roboticslab
{
Expand Down Expand Up @@ -113,7 +113,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC

public:

BasicCartesianControl() : currentState(DEFAULT_INIT_STATE), RateThread(DEFAULT_MS) {}
BasicCartesianControl() : currentState(DEFAULT_INIT_STATE), RateThread(DEFAULT_CMC_RATE_MS) {}

// -- ICartesianControl declarations. Implementation in ICartesianControlImpl.cpp--

Expand Down Expand Up @@ -180,6 +180,10 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC
void handleGcmp();
void handleForc();

bool performDiffInvKin(const std::vector<double> & currentQ,
const std::vector<double> & xdot,
std::vector<double> & qdot);

yarp::dev::PolyDriver solverDevice;
roboticslab::ICartesianSolver *iCartesianSolver;

Expand All @@ -196,6 +200,7 @@ class BasicCartesianControl : public yarp::dev::DeviceDriver, public ICartesianC
double gain;
double maxJointVelocity;
double duration; // [s]
int cmcRateMs;
int numRobotJoints, numSolverJoints;

/** State encoded as a VOCAB which can be stored as an int */
Expand Down
1 change: 1 addition & 0 deletions libraries/YarpPlugins/BasicCartesianControl/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ if(NOT SKIP_BasicCartesianControl)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}) # yarp plugin builder needs this

yarp_add_plugin(BasicCartesianControl BasicCartesianControl.hpp
BasicCartesianControl.cpp
DeviceDriverImpl.cpp
ICartesianControlImpl.cpp
RateThreadImpl.cpp)
Expand Down
1 change: 1 addition & 0 deletions libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ bool roboticslab::BasicCartesianControl::open(yarp::os::Searchable& config) {
gain = config.check("controllerGain",yarp::os::Value(DEFAULT_GAIN),"controller gain").asDouble();
maxJointVelocity = config.check("maxJointVelocity",yarp::os::Value(DEFAULT_QDOT_LIMIT),"maximum joint velocity").asDouble();
duration = config.check("trajectoryDuration",yarp::os::Value(DEFAULT_DURATION),"trajectory duration").asDouble();
cmcRateMs = config.check("cmcRateMs",yarp::os::Value(DEFAULT_CMC_RATE_MS),"CMC rate [ms]").asInt();

std::string referenceFrameStr = config.check("referenceFrame", yarp::os::Value(DEFAULT_REFERENCE_FRAME),
"reference frame").asString();
Expand Down
Loading

0 comments on commit 944ec2a

Please sign in to comment.