Skip to content

Commit

Permalink
Updated controller code to optimize for lower latency calls
Browse files Browse the repository at this point in the history
  • Loading branch information
@burningombre
burningombre committed Mar 30, 2022
1 parent 03bbb6f commit b7b08a2
Show file tree
Hide file tree
Showing 6 changed files with 163 additions and 131 deletions.
22 changes: 8 additions & 14 deletions controller_modules/include/controller_modules/TaskSpaceController.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,42 +7,36 @@
#include "controller_modules/ControllerBase.h"
#include "controller_modules/PDController.h"
#include <iostream>
#include "rbdl_server/RBDLKinimatics.h"
#include "rbdl_server/RBDLPointVelocity.h"
#include "rbdl_server/RBDLJacobian.h"
#include "rbdl_server/RBDLTaskSpaceBody.h"
#include "rbdl_server/RBDLInverseDynamics.h"


class TaskSpaceController : public ControllerBase {

public:
TaskSpaceController(const std::string, ros::NodeHandle*, PDController*);
TaskSpaceController(const std::string, const std::string, ros::NodeHandle*, PDController*);
~TaskSpaceController();

void actual_to_task(const trajectory_msgs::JointTrajectoryPoint& actual, trajectory_msgs::JointTrajectoryPoint& actual_task_space);
void get_Jacobian(const trajectory_msgs::JointTrajectoryPoint& actual, Eigen::MatrixXd &jacobian);
void calculate_xdd(const trajectory_msgs::JointTrajectoryPoint& actual, const trajectory_msgs::JointTrajectoryPoint& desired, Eigen::VectorXd &xdd);
void calculate_aq(const trajectory_msgs::JointTrajectoryPoint& actual, Eigen::VectorXd &xdd, Eigen::VectorXd &aq);

void calculate_torque(const trajectory_msgs::JointTrajectoryPoint& actual, const trajectory_msgs::JointTrajectoryPoint& desired, Eigen::VectorXd &torque);
void update(const trajectory_msgs::JointTrajectoryPoint&, const trajectory_msgs::JointTrajectoryPoint&, std::vector<double>&);

private:
std::string model_name;

std::string ee_body_name;
geometry_msgs::Point ee_point;

PDController my_controller;

ros::NodeHandle nh;
ros::ServiceClient client_fwdKin;
ros::ServiceClient client_ptVel;
ros::ServiceClient client_jacob;

ros::ServiceClient client_taskSpaceBody;
ros::ServiceClient client_invDyn;

ros::Publisher xyz_actual_pub;


void actual_to_task(const trajectory_msgs::JointTrajectoryPoint& actual, trajectory_msgs::JointTrajectoryPoint& actual_task_space, Eigen::MatrixXd &jacobian);
void calculate_xdd(const trajectory_msgs::JointTrajectoryPoint& actual, const trajectory_msgs::JointTrajectoryPoint& desired, Eigen::VectorXd &xdd);

};


Expand Down
155 changes: 41 additions & 114 deletions controller_modules/src/TaskSpaceController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,13 +5,12 @@
*
*
*/
TaskSpaceController::TaskSpaceController(const std::string name, ros::NodeHandle* n, PDController* controller): my_controller(*controller), nh(*n) {
TaskSpaceController::TaskSpaceController(const std::string name, const std::string body, ros::NodeHandle* n, PDController* controller): my_controller(*controller), nh(*n) {

model_name = name;
ee_body_name = body;

client_fwdKin = nh.serviceClient<rbdl_server::RBDLKinimatics>("ForwardKinimatics");
client_ptVel = nh.serviceClient<rbdl_server::RBDLPointVelocity>("PointVelocity");
client_jacob = nh.serviceClient<rbdl_server::RBDLJacobian>("Jacobian");
client_taskSpaceBody = nh.serviceClient<rbdl_server::RBDLTaskSpaceBody>("TaskSpaceBody");
client_invDyn = nh.serviceClient<rbdl_server::RBDLInverseDynamics>("InverseDynamics");

xyz_actual_pub = nh.advertise<std_msgs::Float64MultiArray>("xyz_actual", 1000);
Expand All @@ -29,94 +28,49 @@ TaskSpaceController::~TaskSpaceController() {
* @param actual is the message containing the joint angles/velocities of the model in the generalized space
* @param actual_task_space is the resulting model ee position and velocity in the task space
**/
void TaskSpaceController::actual_to_task(const trajectory_msgs::JointTrajectoryPoint& actual, trajectory_msgs::JointTrajectoryPoint& actual_task_space) {
void TaskSpaceController::actual_to_task(const trajectory_msgs::JointTrajectoryPoint& actual, trajectory_msgs::JointTrajectoryPoint& actual_task_space, Eigen::MatrixXd &jacobian) {

rbdl_server::RBDLKinimatics fwdKin_msg; // Forward kinematics (Base to body coordinates) RBDL server call message
rbdl_server::RBDLTaskSpaceBody taskSpaceBody_msg; // Task space body (pos, vel, and Jacobian of ee) service call message

fwdKin_msg.request.model_name = model_name;
fwdKin_msg.request.q = actual.positions;
taskSpaceBody_msg.request.model_name = model_name;
taskSpaceBody_msg.request.body_name = ee_body_name;
taskSpaceBody_msg.request.q = actual.positions;
taskSpaceBody_msg.request.qd = actual.velocities;

if(client_fwdKin.call(fwdKin_msg)){

std::vector<geometry_msgs::Point> fwdKin_resp_points = fwdKin_msg.response.points; // returns the points in 3D space at the base of the bodies that make up the model
std::vector<std::string> fwdKin_resp_names = fwdKin_msg.response.names;

// TODO don't hardcode this value!
int bodyIndex = 0; // index 0 is "link7"

ee_body_name = fwdKin_resp_names[bodyIndex];
ee_point = fwdKin_resp_points[bodyIndex];
if(client_taskSpaceBody.call(taskSpaceBody_msg)) {

// Position of EE
geometry_msgs::Point ee_point = taskSpaceBody_msg.response.point;
std::vector<double> task_space_pos(3, 0);
task_space_pos[0] = ee_point.x;
task_space_pos[1] = ee_point.y;
task_space_pos[2] = ee_point.z;

// Publishing
std_msgs::Float64MultiArray xyz_actual;
xyz_actual.data = task_space_pos; //std::vector<float>(xyzPosDesired.begin(), xyzPosDesired.end());
xyz_actual_pub.publish(xyz_actual);


actual_task_space.positions = task_space_pos;
printf("EE point: %f, %f, %f\n", ee_point.x, ee_point.y, ee_point.z);

}


rbdl_server::RBDLPointVelocity ptVel_msg; // Point velocity RBDL server call message

ptVel_msg.request.model_name = model_name;
ptVel_msg.request.body_name = ee_body_name;
ptVel_msg.request.q = actual.positions;
ptVel_msg.request.qd = actual.velocities;
ptVel_msg.request.point = ee_point;

if(client_ptVel.call(ptVel_msg)) {

//std::vector<double> ptVel_response(ptVel_msg.response.velocity.begin(), ptVel_msg.response.velocity.end());
//printf("Pt Vel response: %f, %f, %f\n", ptVel_response[3], ptVel_response[4], ptVel_response[5]);

// Velocity (linear part) of EE
std::vector<double> ee_vel_full(taskSpaceBody_msg.response.velocity.begin(), taskSpaceBody_msg.response.velocity.end());
std::vector<double> task_space_vel(3, 0);
task_space_vel[0] = ptVel_msg.response.velocity[3]; // response is translational and rotational velocity (6 values)
task_space_vel[1] = ptVel_msg.response.velocity[4]; // the first three elements are rotational and
task_space_vel[2] = ptVel_msg.response.velocity[5]; // the last three elements are translational

task_space_vel[0] = ee_vel_full[3]; // response is translational and rotational velocity (6 values)
task_space_vel[1] = ee_vel_full[4]; // the first three elements are rotational and
task_space_vel[2] = ee_vel_full[5]; // the last three elements are translational
actual_task_space.velocities = task_space_vel;
//printf("EE velocity: %f, %f, %f\n", ptVel_response[3], ptVel_response[4], ptVel_response[5]);

// Jacobian (linear part) of EE
std_msgs::Float64MultiArray ee_jacobian = taskSpaceBody_msg.response.jacobian;
int nDoF = actual.positions.size();
Eigen::MatrixXd jacobMatFull(Eigen::MatrixXd::Zero(6, nDoF)); // initialize the ee Jacobian matrix (6 x nDoF)
msgToEigenMat(ee_jacobian, jacobMatFull); // Eigen matrix conversion from the message
Eigen::MatrixXd jacobV = jacobMatFull.block(3, 0, 3, nDoF); // split the Jacobian (v and w) to use only v part
jacobian = jacobV;
//printf("EE jacobian: %f\n", ee_jacobian.data[1]);

}


}


/**
* Calculate the current Jacobian matrix for the end-effector given the current manipulator configuration
* @param actual is the message containing the joint angles/velocities of the model in the generalized space
* @param jacobian is the resulting Eigen matrix Jacobian of the current ee position
**/
void TaskSpaceController::get_Jacobian(const trajectory_msgs::JointTrajectoryPoint& actual, Eigen::MatrixXd &jacobian) {

rbdl_server::RBDLJacobian jacob_msg; // Jacobian RBDL server call message

jacob_msg.request.model_name = model_name;
jacob_msg.request.body_name = ee_body_name;
jacob_msg.request.q = actual.positions;
jacob_msg.request.point = ee_point;

if(client_jacob.call(jacob_msg)){

std_msgs::Float64MultiArray jacob_response = jacob_msg.response.jacobian;

int nDoF = jacob_msg.request.q.size();

Eigen::MatrixXd jacobMatFull(Eigen::MatrixXd::Zero(6, nDoF)); // initialize the ee Jacobian matrix (6 x nDoF)
msgToEigenMat(jacob_response, jacobMatFull); // Eigen matrix conversion from the message

// Get the linear velocity part of the Jacobian by removing the angular velocity part when not controlling manipulator orientation
Eigen::MatrixXd jacobV = jacobMatFull.block(3, 0, 3, nDoF); // split the Jacobian (v and w) to use only v part
//std::cout << "Jacob: " << std::endl << jacobV << std::endl;

jacobian = jacobV;
// Publishing
std_msgs::Float64MultiArray xyz_actual;
xyz_actual.data = task_space_pos; //std::vector<float>(xyzPosDesired.begin(), xyzPosDesired.end());
xyz_actual_pub.publish(xyz_actual);

}

Expand Down Expand Up @@ -148,30 +102,6 @@ void TaskSpaceController::calculate_xdd(const trajectory_msgs::JointTrajectoryPo
}


/**
* Function to calculate the aq, the accelerations of the model joints transformed to the generalized space using the Jacobian
* @param actual is the message containing the joint angles/velocities of the model in the generalized space
* @param xdd is the Eigen vector of end-effector accelerations in the task space calculated by the PD controller
* @param aq is the resulting Eigen vector of joint accelerations calculated by the controller
**/
void TaskSpaceController::calculate_aq(const trajectory_msgs::JointTrajectoryPoint& actual, Eigen::VectorXd &xdd, Eigen::VectorXd &aq) {

int nDoF = actual.positions.size(); // number of joints thus degrees of freedom (DoF) of the model

// Calculate the inverse Jacobian matrix
Eigen::MatrixXd jacobMat(Eigen::MatrixXd::Zero(3, nDoF)); // initialize the ee Jacobian matrix (3 x nDoF)
get_Jacobian(actual, jacobMat);

Eigen::MatrixXd invJacobMat = jacobMat.completeOrthogonalDecomposition().pseudoInverse(); // Eigen QR pseudo-inverse of the Jacobian matrix

// TODO test using the Levenberg-Marquardt (damped least squares) algoritm to filter the Jacobian: J = J' * pinv(J * J' + lambda * I), lambda << 1

// Calculate the aq
aq = invJacobMat * xdd;

}


/**
* Function to calculate the resulting torque control input from the task space controller
* @param actual is the message containing the joint angles/velocities of the model in the generalized space
Expand All @@ -180,21 +110,26 @@ void TaskSpaceController::calculate_aq(const trajectory_msgs::JointTrajectoryPoi
**/
void TaskSpaceController::calculate_torque(const trajectory_msgs::JointTrajectoryPoint& actual, const trajectory_msgs::JointTrajectoryPoint& desired, Eigen::VectorXd &torque) {

int nDoF = actual.positions.size(); // number of joints thus degrees of freedom (DoF) of the model
int nDoF = actual.positions.size(); // number of joints thus degrees of freedom (DoF) of the model

// Calculate the task space positions and velocities
// Calculate the task space position, velocity, and Jacobian
trajectory_msgs::JointTrajectoryPoint actual_task;
actual_to_task(actual, actual_task);
Eigen::MatrixXd jacobEE;
actual_to_task(actual, actual_task, jacobEE);

// Calculate the end-effector acceleration
Eigen::VectorXd xdd(Eigen::VectorXd::Zero(3));
calculate_xdd(actual_task, desired, xdd);

// Calculate the aq from the controller
Eigen::VectorXd aq(Eigen::VectorXd::Zero(nDoF));
calculate_aq(actual, xdd, aq);

Eigen::MatrixXd invJacobMat = jacobEE.completeOrthogonalDecomposition().pseudoInverse(); // Eigen QR pseudo-inverse of the Jacobian matrix
// TODO test using the Levenberg-Marquardt (damped least squares) algoritm to filter the Jacobian: J = J' * pinv(J * J' + lambda * I), lambda << 1
aq = invJacobMat * xdd;
std::vector<double> a_q = eigenToVect(aq);


// Calculate the corresponding torque input using inverse dynamics
rbdl_server::RBDLInverseDynamics invDyn_msg;

Expand All @@ -209,14 +144,6 @@ void TaskSpaceController::calculate_torque(const trajectory_msgs::JointTrajector

}


printf("EE point: %f, %f, %f\n", ee_point.x, ee_point.y, ee_point.z);

// printf("resp: %s\n", ee_body_name);
//std::cout << "resp: " << ee_point.x << std::endl;

//std::cout << "resp: " << actual_task.velocities[0] << std::endl;

}


Expand Down
1 change: 1 addition & 0 deletions rbdl_server/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@ add_service_files(
RBDLJointSpaceInertia.srv
RBDLPointVelocity.srv
RBDLNonlinearEffects.srv
RBDLTaskSpaceBody.srv

)

Expand Down
7 changes: 5 additions & 2 deletions rbdl_server/include/rbdl_server/RBDLServer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@
#include "rbdl_server/RBDLJointSpaceInertia.h"
#include "rbdl_server/RBDLPointVelocity.h"
#include "rbdl_server/RBDLNonlinearEffects.h"
#include "rbdl_server/RBDLTaskSpaceBody.h"
#include <iostream>

#include <algorithm>
Expand Down Expand Up @@ -87,7 +88,8 @@ class RBDLServer
std::unordered_map<std::string, std::unordered_map<std::string, unsigned int>> body_ids;


ros::ServiceServer FD_srv, ID_srv, MD_srv, Jac_srv, Kin_srv, InvKin_srv, JointNames_srv, JointAlign_srv, Mjoint_srv, PtVel_srv, NonlinEff_srv;
ros::ServiceServer FD_srv, ID_srv, MD_srv, Jac_srv, Kin_srv, InvKin_srv, JointNames_srv, JointAlign_srv, Mjoint_srv, PtVel_srv, NonlinEff_srv,
TaskSpBd_srv;
VectorNd VectToEigen(const std::vector<double>&);
RigidBodyDynamics::Model* getModel(std::string);
bool CreateModel_srv(rbdl_server::RBDLModelRequest&, rbdl_server::RBDLModelResponse& ); //parses the AMBF model into rbdl model
Expand All @@ -105,10 +107,11 @@ class RBDLServer
bool ForwardKinimatics_srv(rbdl_server::RBDLKinimaticsRequest&, rbdl_server::RBDLKinimaticsResponse&);
bool InverseKinimatics_srv(rbdl_server::RBDLInverseKinimaticsRequest&, rbdl_server::RBDLInverseKinimaticsResponse& );
bool Jacobian_srv(rbdl_server::RBDLJacobianRequest&, rbdl_server::RBDLJacobianResponse&);

bool JointSpaceInertia_srv(rbdl_server::RBDLJointSpaceInertiaRequest&, rbdl_server::RBDLJointSpaceInertiaResponse&);
bool PointVelocity_srv(rbdl_server::RBDLPointVelocityRequest&, rbdl_server::RBDLPointVelocityResponse&);
bool NonlinearEffects_srv(rbdl_server::RBDLNonlinearEffectsRequest&, rbdl_server::RBDLNonlinearEffectsResponse&);

bool TaskSpaceBody_srv(rbdl_server::RBDLTaskSpaceBodyRequest&, rbdl_server::RBDLTaskSpaceBodyResponse&);


public:
Expand Down
Loading

0 comments on commit b7b08a2

Please sign in to comment.