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);
}
Esempio n. 3
0
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();


}
Esempio n. 4
0
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);
	}
}
Esempio n. 5
0
 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);
        }
    }
 }
Esempio n. 6
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 ());
    }
Esempio n. 7
0
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");
		}
	}
}