bool MotomanJointTrajectoryStreamer::VectorToJointData(const std::vector<double> &vec,
JointData &joints)
{
if (vec.size() > joints.getMaxNumJoints())
ROS_ERROR_RETURN(false, "Failed to copy to JointData. Len (%d) out of range (0 to %d)",
vec.size(), joints.getMaxNumJoints());
joints.init();
for (int i = 0; i < vec.size(); ++i)
{
joints.setJoint(i, vec[i]);
}
return true;
}
void JointDataTest::instantiationTest() {
std::string id = "id1234";
float ang = 1232.023;
bool success = true;
JointData j = JointData(id, ang);
if(j.getAngle() != ang)success = false;
std::cout << "Step 1. Angle: " << j.getAngle() << std::endl;
if(j.getJointId() != id)success = false;
std::cout << "Step 2. ID: " << j.getJointId() << std::endl;
CPPUNIT_ASSERT(success);
}
TEST(DISABLED_TrajectoryJob, init)
{
using namespace std;
TrajectoryJob job;
JointTraj traj;
JointTrajPt pt;
JointData init;
string job_name = "JOB_NAME";
string job_ext = ".JBI";
const int TRAJ_SIZE = 50;
const int BIG_JOB_BUFFER_SIZE = 500000;
char bigJobBuffer[BIG_JOB_BUFFER_SIZE];
const int SMALL_JOB_BUFFER_SIZE = 10;
char smallJobBuffer[SMALL_JOB_BUFFER_SIZE];
for (int i = 1; i <= TRAJ_SIZE; i++)
{
//ROS_DEBUG("Initializing joint: %d", i);
for (int j = 0; j < init.getMaxNumJoints(); j++)
{
//ROS_DEBUG("Initializing point: %d.%d", i, j);
ASSERT_TRUE(init.setJoint(j, (i*(j+1))));
}
pt.init(i, init, 11.1111, 0.01);
ASSERT_TRUE(traj.addPoint(pt));
}
EXPECT_FALSE(job.init("this name is way too long to be the name of a file and this should fail"));
ASSERT_TRUE(job.init((char*)job_name.c_str()));
EXPECT_FALSE(job.toJobString(traj, &smallJobBuffer[0], SMALL_JOB_BUFFER_SIZE));
// Test is disabled because the line below fails due to missing conversions functions. This
// ability has been moved to the controller and now is dynamically updated at runtime. There
// may not be a solution to this issue.
EXPECT_TRUE(job.toJobString(traj, &bigJobBuffer[0], BIG_JOB_BUFFER_SIZE));
ofstream file;
job_name.append(job_ext);
file.open(job_name.c_str());
ASSERT_TRUE(file.is_open());
file << bigJobBuffer;
file.close();
}
void PreviewWalk::plotJointAngles(const JointData& joints, const std::string& prefix)
{
std::string name;
for(int i = 1; i < JointData::numOfJoints; ++i)
{
name = prefix + joints.getJointName(JointData::Joint(i));
//MATLABHELP_PLOT(joints.angles[i], name);
}
}
void JointMotionHandler::motionInterface(JointMessage & jMsg)
{
//
// Upon receiving...
// 1st point - initialize buffer, enable motion, start job, add point (increment buffer index),
// Nth point - add point (increment buffer index)
// end of trajectory - wait until buffer size = 0, disable motion, stop job, reset buffer indicies
//motion stop - disable motion, stop job
JointData joints;
switch (jMsg.getSequence())
{
case SpecialSeqValues::END_TRAJECTORY:
LOG_INFO("Received end trajectory command");
while(!pVarQ.bufferEmpty())
{
LOG_DEBUG("Waiting for motion buffer to empty");
mpTaskDelay(this->BUFFER_POLL_TICK_DELAY);
};
this->stopMotionJob();
break;
case SpecialSeqValues::STOP_TRAJECTORY:
LOG_WARN("Received stop command, stopping motion immediately");
this->stopMotionJob();
break;
default:
joints.copyFrom(jMsg.getJoints());
if (!(this->isJobStarted()))
{
//TODO: The velocity should be set from the message in the future.
pVarQ.init(joints, 0.0);
this->startMotionJob();
}
else
{
pVarQ.addPoint(joints, 0.0);
}
}
}
void calc (JointData & data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
{
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ> (idx_q ());
data.v () = vs.segment<NV> (idx_v ());
const JointData::Quaternion quat(Eigen::Matrix<double,4,1> (q.tail <4> ())); // TODO
data.M.rotation (quat.matrix ());
}
int MultiBodyTree::finalize() {
const int &num_bodies = m_init_cache->numBodies();
const int &num_dofs = m_init_cache->numDoFs();
if(num_dofs<=0) {
error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
//return -1;
}
// 1 allocate internal MultiBody structure
m_impl = new MultiBodyImpl(num_bodies, num_dofs);
// 2 build new index set assuring index(parent) < index(child)
if (-1 == m_init_cache->buildIndexSets()) {
return -1;
}
m_init_cache->getParentIndexArray(&m_impl->m_parent_index);
// 3 setup internal kinematic and dynamic data
for (int index = 0; index < num_bodies; index++) {
InertiaData inertia;
JointData joint;
if (-1 == m_init_cache->getInertiaData(index, &inertia)) {
return -1;
}
if (-1 == m_init_cache->getJointData(index, &joint)) {
return -1;
}
RigidBody &rigid_body = m_impl->m_body_list[index];
rigid_body.m_mass = inertia.m_mass;
rigid_body.m_body_mass_com = inertia.m_mass * inertia.m_body_pos_body_com;
rigid_body.m_body_I_body = inertia.m_body_I_body;
rigid_body.m_joint_type = joint.m_type;
rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
rigid_body.m_body_T_parent_ref = joint.m_child_T_parent_ref;
rigid_body.m_parent_pos_parent_body_ref = joint.m_parent_pos_parent_child_ref;
rigid_body.m_joint_type = joint.m_type;
// Set joint Jacobians. Note that the dimension is always 3x1 here to avoid variable sized
// matrices.
switch (rigid_body.m_joint_type) {
case REVOLUTE:
rigid_body.m_Jac_JR(0) = joint.m_child_axis_of_motion(0);
rigid_body.m_Jac_JR(1) = joint.m_child_axis_of_motion(1);
rigid_body.m_Jac_JR(2) = joint.m_child_axis_of_motion(2);
rigid_body.m_Jac_JT(0) = 0.0;
rigid_body.m_Jac_JT(1) = 0.0;
rigid_body.m_Jac_JT(2) = 0.0;
break;
case PRISMATIC:
rigid_body.m_Jac_JR(0) = 0.0;
rigid_body.m_Jac_JR(1) = 0.0;
rigid_body.m_Jac_JR(2) = 0.0;
rigid_body.m_Jac_JT(0) = joint.m_child_axis_of_motion(0);
rigid_body.m_Jac_JT(1) = joint.m_child_axis_of_motion(1);
rigid_body.m_Jac_JT(2) = joint.m_child_axis_of_motion(2);
break;
case FIXED:
// NOTE/TODO: dimension really should be zero ..
rigid_body.m_Jac_JR(0) = 0.0;
rigid_body.m_Jac_JR(1) = 0.0;
rigid_body.m_Jac_JR(2) = 0.0;
rigid_body.m_Jac_JT(0) = 0.0;
rigid_body.m_Jac_JT(1) = 0.0;
rigid_body.m_Jac_JT(2) = 0.0;
break;
case FLOATING:
// NOTE/TODO: this is not really correct.
// the Jacobians should be 3x3 matrices here !
rigid_body.m_Jac_JR(0) = 0.0;
rigid_body.m_Jac_JR(1) = 0.0;
rigid_body.m_Jac_JR(2) = 0.0;
rigid_body.m_Jac_JT(0) = 0.0;
rigid_body.m_Jac_JT(1) = 0.0;
rigid_body.m_Jac_JT(2) = 0.0;
break;
default:
error_message("unsupported joint type %d\n", rigid_body.m_joint_type);
return -1;
}
}
// 4 assign degree of freedom indices & build per-joint-type index arrays
if (-1 == m_impl->generateIndexSets()) {
error_message("generating index sets\n");
return -1;
}
// 5 do some pre-computations ..
m_impl->calculateStaticData();
// 6. make sure all user forces are set to zero, as this might not happen
// in the vector ctors.
m_impl->clearAllUserForcesAndMoments();
m_is_finalized = true;
return 0;
}
void JointTrajectoryDownloader::jointTrajectoryCB(
const trajectory_msgs::JointTrajectoryConstPtr &msg)
{
ROS_INFO("Receiving joint trajectory message");
/*
if (!checkTrajectory(msg))
{
ROS_ERROR("Joint trajectory check failed, trajectory not downloaded");
return;
}
std::vector<double> joint_velocity_limits;
if (!getVelocityLimits("robot_description", msg->joint_names, joint_velocity_limits))
{
ROS_ERROR("Failed to get joint velocity limits");
return;
}
*/
for (int i = 0; i < msg->points.size(); i++)
{
ROS_INFO("Sending joints trajectory point[%d]", i);
JointTrajPt jPt;
JointTrajPtMessage jMsg;
SimpleMessage topic;
trajectory_msgs::JointTrajectoryPoint pt;
pt = msg->points[i];
// The first and last sequence values must be given a special sequence
// value
if (0 == i)
{
ROS_DEBUG("First trajectory point, setting special sequence value");
jPt.setSequence(SpecialSeqValues::START_TRAJECTORY_DOWNLOAD);
}
else if (msg->points.size() - 1 == i)
{
ROS_DEBUG("Last trajectory point, setting special sequence value");
jMsg.point_.setSequence(SpecialSeqValues::END_TRAJECTORY);
}
else
{
jMsg.point_.setSequence(i);
}
// Copy position data to local variable
JointData data;
for (int j = 0; j < msg->joint_names.size(); j++)
{
pt = msg->points[j];
data.setJoint(j, pt.positions[j]);
}
// Initialize joint trajectory message
jPt.setJointPosition(data);
jMsg.init(jPt);
jMsg.toTopic(topic);
ROS_DEBUG("Sending joint trajectory point");
if (this->robot_->sendMsg(topic))
{
ROS_INFO("Point[%d] sent to controller", i);
}
else
{
ROS_WARN("Failed sent joint point, skipping point");
}
}
}
void JointData::mirrorFrom(const JointData& jointData)
{
for (int i = 0; i < numOfJoint; i++)
position[i] = jointData.mirrorData((JointID) i);
}//end mirror
void JointTrajectoryDownloader::jointTrajectoryCB(
const trajectory_msgs::JointTrajectoryConstPtr &msg)
{
ROS_INFO("Receiving joint trajectory message");
// TBD: re-enable in a more generic fashion?
//if (!checkTrajectory(msg))
//{
// ROS_ERROR("Joint trajectory check failed, trajectory not downloaded");
// return;
//}
std::vector<double> joint_velocity_limits;
std::vector<trajectory_msgs::JointTrajectoryPoint> points(msg->points);
// TBD: re-enable velocity scaling
//if (!getVelocityLimits("robot_description", msg, joint_velocity_limits))
//{
// ROS_ERROR("Failed to get joint velocity limits");
// return;
//}
// Trajectory download requires at least two points (START/END)
if (points.size() < 2)
points.push_back(trajectory_msgs::JointTrajectoryPoint(points[0]));
if (!this->robot_->isConnected())
{
ROS_WARN("Attempting robot reconnection");
this->robot_->makeConnect();
}
ROS_INFO("Sending trajectory points, size: %d", points.size());
for (int i = 0; i < points.size(); i++)
{
ROS_INFO("Sending joints trajectory point[%d]", i);
JointTrajPt jPt;
JointTrajPtMessage jMsg;
SimpleMessage topic;
// Performing a manual copy of the joint velocities in order to send them
// to the utility function. Passing the pt data members doesn't seem to
// work.
std::vector<double> joint_velocities(0.0);
double velocity =0 ;
joint_velocities.resize(msg->joint_names.size(), 0.0);
for (int j = 0; j < joint_velocities.size(); j++)
{
joint_velocities[j] = points[i].velocities[j];
}
ROS_DEBUG("Joint velocities copied");
// TBD: restore joint-velocity scaling
//velocity = toMotomanVelocity(joint_velocity_limits, joint_velocities);
velocity = 0.20; // TBD: hardcode for now
jPt.setVelocity(velocity);
// The first and last sequence values must be given a special sequence
// value
if (0 == i)
{
ROS_DEBUG("First trajectory point, setting special sequence value");
jPt.setSequence(SpecialSeqValues::START_TRAJECTORY_DOWNLOAD);
}
else if (points.size() - 1 == i)
{
ROS_DEBUG("Last trajectory point, setting special sequence value");
jPt.setSequence(SpecialSeqValues::END_TRAJECTORY);
}
else
{
jPt.setSequence(i);
}
// Copy position data to local variable
JointData data;
for (int j = 0; j < msg->joint_names.size(); j++)
{
data.setJoint(j, points[i].positions[j]);
}
// Initialize joint trajectory message
jPt.setJointPosition(data);
jMsg.init(jPt);
jMsg.toTopic(topic);
ROS_DEBUG("Sending joint trajectory point");
if (this->robot_->sendMsg(topic))
{
ROS_INFO("Point[%d] sent to controller", i);
}
else
{
ROS_WARN("Failed sent joint point, skipping point");
}
}
}
