open_manipulator_controller_pkg - 8BitsCoding/RobotMentor GitHub Wiki

void OpenManipulatorController::initServer()
{
    // ...
    goal_joint_space_path_from_present_server_      = priv_node_handle_.advertiseService("goal_joint_space_path_from_present", &OpenManipulatorController::goalJointSpacePathFromPresentCallback, this);
    // ...
}

bool OpenManipulatorController::goalJointSpacePathFromPresentCallback(open_manipulator_msgs::SetJointPosition::Request  &req,
                                                                      open_manipulator_msgs::SetJointPosition::Response &res)
{
  std::vector <double> target_angle;

  for(int i = 0; i < req.joint_position.joint_name.size(); i ++)
    target_angle.push_back(req.joint_position.position.at(i));

  open_manipulator_.makeJointTrajectoryFromPresentPosition(target_angle, req.path_time);

  res.is_planned = true;
  return true;
}

class OpenManipulatorController
{
    // ...
    // Related robotis_manipulator
    OpenManipulator open_manipulator_;
    // ...
}

class OpenManipulator : public robotis_manipulator::RobotisManipulator

void RobotisManipulator::makeJointTrajectoryFromPresentPosition(std::vector<double> delta_goal_joint_position, double move_time, std::vector<JointValue> present_joint_value)
{
  if(present_joint_value.size() != 0)
  {
    trajectory_.setPresentJointWaypoint(present_joint_value);
    trajectory_.updatePresentWaypoint(kinematics_);
  }

  JointWaypoint present_way_point = trajectory_.getPresentJointWaypoint();
  std::vector<double> goal_joint_position;
  for(int i = 0; i < trajectory_.getManipulator()->getDOF(); i ++)
    goal_joint_position.push_back(present_way_point.at(i).position + delta_goal_joint_position.at(i));

  makeJointTrajectory(goal_joint_position, move_time);
}

void RobotisManipulator::makeJointTrajectory(std::vector<double> goal_joint_position, double move_time, std::vector<JointValue> present_joint_value)
{
  trajectory_.setTrajectoryType(JOINT_TRAJECTORY);
  trajectory_.setMoveTime(move_time);

  if(present_joint_value.size() != 0)
  {
    trajectory_.setPresentJointWaypoint(present_joint_value);
    trajectory_.updatePresentWaypoint(kinematics_);
  }

  JointWaypoint present_way_point = trajectory_.getPresentJointWaypoint();

  JointValue goal_way_point_temp;
  JointWaypoint goal_way_point;
  for (uint8_t index = 0; index < trajectory_.getManipulator()->getDOF(); index++)
  {
    goal_way_point_temp.position = goal_joint_position.at(index);
    goal_way_point_temp.velocity = 0.0;
    goal_way_point_temp.acceleration = 0.0;
    goal_way_point_temp.effort = 0.0;

    goal_way_point.push_back(goal_way_point_temp);
  }

  if(getMovingState())
  {
    moving_state_=false;
    while(!step_moving_state_);
  }
  trajectory_.makeJointTrajectory(present_way_point, goal_way_point);
  startMoving();
}

void RobotisManipulator::startMoving()      //Private
{
  moving_state_ = true;
  trajectory_.setStartTimeToPresentTime();
}

// h
class RobotisManipulator
{
private:
  Manipulator manipulator_;
  Trajectory trajectory_;
  // ...

void JointTrajectory::makeJointTrajectory(double move_time, JointWaypoint start,
                           JointWaypoint goal)
{
  coefficient_size_ = start.size();
  minimum_jerk_coefficient_.resize(6,coefficient_size_);
  for (uint8_t index = 0; index < coefficient_size_; index++)
  {
    minimum_jerk_trajectory_generator_.calcCoefficient(start.at(index),
                                    goal.at(index),
                                    move_time);

    minimum_jerk_coefficient_.col(index) = minimum_jerk_trajectory_generator_.getCoefficient();
  }
}

// h
class TaskTrajectory
{
private:
  uint8_t coefficient_size_;
  MinimumJerk minimum_jerk_trajectory_generator_;
  Eigen::MatrixXd minimum_jerk_coefficient_;

namespace robotis_manipulator
{
class MinimumJerk
{
private:
  Eigen::VectorXd coefficient_;

public:
  MinimumJerk();
  virtual ~MinimumJerk();

  void calcCoefficient(Point start,
                       Point goal,
                       double move_time);

  Eigen::VectorXd getCoefficient();
};

void MinimumJerk::calcCoefficient(Point start,
                                  Point goal,
                                  double move_time)
{
  Eigen::Matrix3d A = Eigen::Matrix3d::Identity(3, 3);
  Eigen::Vector3d x = Eigen::Vector3d::Zero();
  Eigen::Vector3d b = Eigen::Vector3d::Zero();

  A << pow(move_time, 3), pow(move_time, 4), pow(move_time, 5),
      3 * pow(move_time, 2), 4 * pow(move_time, 3), 5 * pow(move_time, 4),
      6 * pow(move_time, 1), 12 * pow(move_time, 2), 20 * pow(move_time, 3);

  coefficient_(0) = start.position;
  coefficient_(1) = start.velocity;
  coefficient_(2) = 0.5 * start.acceleration;

  b << (goal.position - start.position - (start.velocity * move_time + 0.5 * start.acceleration * pow(move_time, 2))),
      (goal.velocity - start.velocity - (start.acceleration * move_time)),
      (goal.acceleration - start.acceleration);

  Eigen::ColPivHouseholderQR<Eigen::Matrix3d> dec(A);
  x = dec.solve(b);

  coefficient_(3) = x(0);
  coefficient_(4) = x(1);
  coefficient_(5) = x(2);
}

bool RobotisManipulator::sendJointActuatorValue(Name joint_component_name, JointValue value)
{
  // trajectory manipulator set
  // trajectory_.getManipulator()->setJointValue(joint_component_name,value);
  // trajectory_.updatePresentWayPoint(kinematics_dynamics_);

  if(actuator_added_stete_)
  {
    double coefficient = manipulator_.getCoefficient(joint_component_name);
    value.position = value.position / coefficient;
    value.velocity = value.velocity / coefficient;
    value.acceleration = value.acceleration / coefficient;
    value.effort = value.effort;

    std::vector<uint8_t> id;
    std::vector<JointValue> value_vector;
    id.push_back(manipulator_.getId(joint_component_name));
    value_vector.push_back(value);

    //send to actuator
    return joint_actuator_.at(manipulator_.getComponentActuatorName(joint_component_name))->sendJointActuatorValue(id, value_vector);
  }
  else
  {
    manipulator_.setJointValue(joint_component_name, value);
    return true;
  }
  return false;
}

class RobotisManipulator
{
private:
  // ...
  std::map<Name, JointActuator *> joint_actuator_;

// robotis_manipulator_manager.h
class JointActuator
{
public:
  bool enabled_state_;

  JointActuator() : enabled_state_(false) {}
  virtual ~JointActuator() {}

  virtual void init(std::vector<uint8_t> actuator_id, const void *arg) = 0;
  virtual void setMode(std::vector<uint8_t> actuator_id, const void *arg) = 0;
  virtual std::vector<uint8_t> getId() = 0;

  virtual void enable() = 0;
  virtual void disable() = 0;

  virtual bool sendJointActuatorValue(std::vector<uint8_t> actuator_id, std::vector<ActuatorValue> value_vector) = 0;
  virtual std::vector<ActuatorValue> receiveJointActuatorValue(std::vector<uint8_t> actuator_id) = 0;

  bool findId(uint8_t actuator_id);
  bool getEnabledState();
};

// dynamixel.h
class JointDynamixel : public robotis_manipulator::JointActuator
{

bool JointDynamixel::sendJointActuatorValue(std::vector<uint8_t> actuator_id, std::vector<robotis_manipulator::ActuatorValue> value_vector)
{
  bool result = false;

  std::vector<double> radian_vector;
  for(uint32_t index = 0; index < value_vector.size(); index++)
  {
    radian_vector.push_back(value_vector.at(index).position);
  }
  result = JointDynamixel::writeGoalPosition(actuator_id, radian_vector);
  if (result == false)
    return false;

  return true;
}

bool JointDynamixel::writeGoalPosition(std::vector<uint8_t> actuator_id, std::vector<double> radian_vector)
{
  bool result = false;
  const char* log = NULL;

  uint8_t id_array[actuator_id.size()];
  int32_t goal_position[actuator_id.size()];

  for (uint8_t index = 0; index < actuator_id.size(); index++)
  {
    id_array[index] = actuator_id.at(index);
    goal_position[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), radian_vector.at(index));
  }

  result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_position, 1, &log);
  if (result == false)
  {
    log::error(log);
  }

  return true;
}