robotis_manipulator::RobotisManipulator Class Reference

The RobotisManipulator class. More...

#include <robotis_manipulator.h>

Collaboration diagram for robotis_manipulator::RobotisManipulator:

Public Member Functions
	RobotisManipulator ()
virtual	~RobotisManipulator ()
void	addWorld (Name world_name, Name child_name, Eigen::Vector3d world_position=Eigen::Vector3d::Zero(), Eigen::Matrix3d world_orientation=Eigen::Matrix3d::Identity())
	addWorld More...
void	addJoint (Name my_name, Name parent_name, Name child_name, Eigen::Vector3d relative_position, Eigen::Matrix3d relative_orientation, Eigen::Vector3d axis_of_rotation=Eigen::Vector3d::Zero(), int8_t joint_actuator_id=-1, double max_position_limit=M_PI, double min_position_limit=-M_PI, double coefficient=1.0, double mass=0.0, Eigen::Matrix3d inertia_tensor=Eigen::Matrix3d::Identity(), Eigen::Vector3d center_of_mass=Eigen::Vector3d::Zero())
	addJoint More...
void	addTool (Name my_name, Name parent_name, Eigen::Vector3d relative_position, Eigen::Matrix3d relative_orientation, int8_t tool_id=-1, double max_position_limit=M_PI, double min_position_limit=-M_PI, double coefficient=1.0, double mass=0.0, Eigen::Matrix3d inertia_tensor=Eigen::Matrix3d::Identity(), Eigen::Vector3d center_of_mass=Eigen::Vector3d::Zero())
	addTool More...
void	addComponentChild (Name my_name, Name child_name)
	addComponentChild More...
void	printManipulatorSetting ()
	printManipulatorSetting More...
void	addKinematics (Kinematics *kinematics)
	addKinematics More...
void	addJointActuator (Name actuator_name, JointActuator *joint_actuator, std::vector< uint8_t > id_array, const void *arg)
	addJointActuator More...
void	addToolActuator (Name tool_name, ToolActuator *tool_actuator, uint8_t id, const void *arg)
	addToolActuator More...
void	addCustomTrajectory (Name trajectory_name, CustomJointTrajectory *custom_trajectory)
	addCustomTrajectory More...
void	addCustomTrajectory (Name trajectory_name, CustomTaskTrajectory *custom_trajectory)
	addCustomTrajectory More...
Manipulator *	getManipulator ()
	getManipulator More...
JointValue	getJointValue (Name joint_name)
	getJointValue More...
JointValue	getToolValue (Name tool_name)
	getToolValue More...
std::vector< JointValue >	getAllActiveJointValue ()
	getAllActiveJointValue More...
std::vector< JointValue >	getAllJointValue ()
	getAllJointValue More...
std::vector< double >	getAllToolPosition ()
	getAllToolPosition More...
std::vector< JointValue >	getAllToolValue ()
	getAllToolValue More...
KinematicPose	getKinematicPose (Name component_name)
	getKinematicPose More...
DynamicPose	getDynamicPose (Name component_name)
	getDynamicPose More...
Pose	getPose (Name component_name)
	getPose More...
Eigen::MatrixXd	jacobian (Name tool_name)
	jacobian More...
void	solveForwardKinematics ()
	solveForwardKinematics More...
bool	solveInverseKinematics (Name tool_name, Pose goal_pose, std::vector< JointValue > *goal_joint_value)
	solveInverseKinematics More...
void	setKinematicsOption (const void *arg)
	setKinematicsOption More...
void	setJointActuatorMode (Name actuator_name, std::vector< uint8_t > id_array, const void *arg)
	setJointActuatorMode More...
void	setToolActuatorMode (Name actuator_name, const void *arg)
	setToolActuatorMode More...
std::vector< uint8_t >	getJointActuatorId (Name actuator_name)
	getJointActuatorId More...
uint8_t	getToolActuatorId (Name actuator_name)
	getToolActuatorId More...
void	enableActuator (Name actuator_name)
	enableActuator More...
void	disableActuator (Name actuator_name)
	disableActuator More...
void	enableAllJointActuator ()
	enableAllJointActuator More...
void	disableAllJointActuator ()
	disableAllJointActuator More...
void	enableAllToolActuator ()
	enableAllToolActuator More...
void	disableAllToolActuator ()
	disableAllToolActuator More...
void	enableAllActuator ()
	enableAllActuator More...
void	disableAllActuator ()
	disableAllActuator More...
bool	getActuatorEnabledState (Name actuator_name)
	getActuatorEnabledState More...
bool	sendJointActuatorValue (Name joint_component_name, JointValue value)
	sendJointActuatorValue More...
bool	sendMultipleJointActuatorValue (std::vector< Name > joint_component_name, std::vector< JointValue > value_vector)
	sendMultipleJointActuatorValue More...
bool	sendAllJointActuatorValue (std::vector< JointValue > value_vector)
	sendAllJointActuatorValue More...
JointValue	receiveJointActuatorValue (Name joint_component_name)
	receiveJointActuatorValue More...
std::vector< JointValue >	receiveMultipleJointActuatorValue (std::vector< Name > joint_component_name)
	receiveMultipleJointActuatorValue More...
std::vector< JointValue >	receiveAllJointActuatorValue ()
	receiveAllJointActuatorValue More...
bool	sendToolActuatorValue (Name tool_component_name, JointValue value)
	sendToolActuatorValue More...
bool	sendMultipleToolActuatorValue (std::vector< Name > tool_component_name, std::vector< JointValue > value_vector)
	sendMultipleToolActuatorValue More...
bool	sendAllToolActuatorValue (std::vector< JointValue > value_vector)
	sendAllToolActuatorValue More...
JointValue	receiveToolActuatorValue (Name tool_component_name)
	receiveToolActuatorValue More...
std::vector< JointValue >	receiveMultipleToolActuatorValue (std::vector< Name > tool_component_name)
	receiveMultipleToolActuatorValue More...
std::vector< JointValue >	receiveAllToolActuatorValue ()
	receiveAllToolActuatorValue More...
double	getTrajectoryMoveTime ()
	getTrajectoryMoveTime More...
bool	getMovingState ()
	getMovingState More...
bool	checkJointLimit (Name component_name, double position)
	checkJointLimit More...
bool	checkJointLimit (Name component_name, JointValue value)
	checkJointLimit More...
bool	checkJointLimit (std::vector< Name > component_name, std::vector< double > position_vector)
	checkJointLimit More...
bool	checkJointLimit (std::vector< Name > component_name, std::vector< JointValue > value_vector)
	checkJointLimit More...
Trajectory *	getTrajectory ()
	getTrajectory More...
void	makeJointTrajectoryFromPresentPosition (std::vector< double > delta_goal_joint_position, double move_time, std::vector< JointValue > present_joint_value={})
	makeJointTrajectoryFromPresentPosition More...
void	makeJointTrajectory (std::vector< double > goal_joint_position, double move_time, std::vector< JointValue > present_joint_value={})
	makeJointTrajectory More...
void	makeJointTrajectory (std::vector< JointValue > goal_joint_value, double move_time, std::vector< JointValue > present_joint_value={})
	makeJointTrajectory More...
void	makeJointTrajectory (Name tool_name, Eigen::Vector3d goal_position, double move_time, std::vector< JointValue > present_joint_value={})
	makeJointTrajectory More...
void	makeJointTrajectory (Name tool_name, Eigen::Matrix3d goal_orientation, double move_time, std::vector< JointValue > present_joint_value={})
	makeJointTrajectory More...
void	makeJointTrajectory (Name tool_name, KinematicPose goal_pose, double move_time, std::vector< JointValue > present_joint_value={})
	makeJointTrajectory More...
void	makeTaskTrajectoryFromPresentPose (Name tool_name, Eigen::Vector3d position_meter, double move_time, std::vector< JointValue > present_joint_value={})
	makeTaskTrajectoryFromPresentPose More...
void	makeTaskTrajectoryFromPresentPose (Name tool_name, Eigen::Matrix3d orientation_meter, double move_time, std::vector< JointValue > present_joint_value={})
	makeTaskTrajectoryFromPresentPose More...
void	makeTaskTrajectoryFromPresentPose (Name tool_name, KinematicPose goal_pose_delta, double move_time, std::vector< JointValue > present_joint_value={})
	makeTaskTrajectoryFromPresentPose More...
void	makeTaskTrajectory (Name tool_name, Eigen::Vector3d goal_position, double move_time, std::vector< JointValue > present_joint_value={})
	makeTaskTrajectory More...
void	makeTaskTrajectory (Name tool_name, Eigen::Matrix3d goal_orientation, double move_time, std::vector< JointValue > present_joint_value={})
	makeTaskTrajectory More...
void	makeTaskTrajectory (Name tool_name, KinematicPose goal_pose, double move_time, std::vector< JointValue > present_joint_value={})
	makeTaskTrajectory More...
void	setCustomTrajectoryOption (Name trajectory_name, const void *arg)
	setCustomTrajectoryOption More...
void	makeCustomTrajectory (Name trajectory_name, Name tool_name, const void *arg, double move_time, std::vector< JointValue > present_joint_value={})
	makeCustomTrajectory More...
void	makeCustomTrajectory (Name trajectory_name, const void *arg, double move_time, std::vector< JointValue > present_joint_value={})
	makeCustomTrajectory More...
void	sleepTrajectory (double wait_time, std::vector< JointValue > present_joint_value={})
	sleepTrajectory More...
void	makeToolTrajectory (Name tool_name, double tool_goal_position)
	makeToolTrajectory More...
std::vector< JointValue >	getJointGoalValueFromTrajectory (double present_time)
	getJointGoalValueFromTrajectory More...
std::vector< JointValue >	getToolGoalValue ()
	getToolGoalValue More...
std::vector< JointValue >	getJointGoalValueFromTrajectoryTickTime (double tick_time)
	getJointGoalValueFromTrajectoryTickTime More...

Private Member Functions
void	startMoving ()
	startMoving More...
JointWaypoint	getTrajectoryJointValue (double tick_time)
	getTrajectoryJointValue More...

Private Attributes
Manipulator	manipulator_
Trajectory	trajectory_
Kinematics *	kinematics_
std::map< Name, JointActuator * >	joint_actuator_
std::map< Name, ToolActuator * >	tool_actuator_
bool	trajectory_initialized_state_
bool	actuator_added_stete_
bool	moving_state_
bool	step_moving_state_

Detailed Description

The RobotisManipulator class.

Definition at line 44 of file robotis_manipulator.h.

Constructor & Destructor Documentation

RobotisManipulator::RobotisManipulator

(

)

Definition at line 33 of file robotis_manipulator.cpp.

{
  moving_state_ = false;
  actuator_added_stete_ = false;
  step_moving_state_ = false;
  trajectory_initialized_state_ = false;
}

RobotisManipulator::~RobotisManipulator ( )

virtual

Definition at line 41 of file robotis_manipulator.cpp.

{}

Member Function Documentation

void RobotisManipulator::addComponentChild	(	Name	my_name,
		Name	child_name
	)

addComponentChild

Parameters

my_name
child_name

Definition at line 76 of file robotis_manipulator.cpp.

{
  manipulator_.addComponentChild(my_name, child_name);
}

Here is the call graph for this function:

void RobotisManipulator::addCustomTrajectory	(	Name	trajectory_name,
		CustomJointTrajectory *	custom_trajectory
	)

addCustomTrajectory

Parameters

trajectory_name
custom_trajectory

Definition at line 142 of file robotis_manipulator.cpp.

{
  trajectory_.addCustomTrajectory(trajectory_name, custom_trajectory);
}

Here is the call graph for this function:

void RobotisManipulator::addCustomTrajectory	(	Name	trajectory_name,
		CustomTaskTrajectory *	custom_trajectory
	)

addCustomTrajectory

Parameters

trajectory_name
custom_trajectory

Definition at line 147 of file robotis_manipulator.cpp.

{
  trajectory_.addCustomTrajectory(trajectory_name, custom_trajectory);
}

Here is the call graph for this function:

void RobotisManipulator::addJoint	(	Name	my_name,
		Name	parent_name,
		Name	child_name,
		Eigen::Vector3d	relative_position,
		Eigen::Matrix3d	relative_orientation,
		Eigen::Vector3d	axis_of_rotation = Eigen::Vector3d::Zero(),
		int8_t	joint_actuator_id = -1,
		double	max_position_limit = M_PI,
		double	min_position_limit = -M_PI,
		double	coefficient = 1.0,
		double	mass = 0.0,
		Eigen::Matrix3d	inertia_tensor = Eigen::Matrix3d::Identity(),
		Eigen::Vector3d	center_of_mass = Eigen::Vector3d::Zero()
	)

addJoint

Parameters

my_name
parent_name
child_name
relative_position
relative_orientation
axis_of_rotation
joint_actuator_id
max_position_limit
min_position_limit
coefficient
mass
inertia_tensor
center_of_mass

Definition at line 56 of file robotis_manipulator.cpp.

{
  manipulator_.addJoint(my_name, parent_name, child_name, 
                        relative_position, relative_orientation, axis_of_rotation, joint_actuator_id, 
                        max_position_limit, min_position_limit, coefficient, mass, 
                        inertia_tensor, center_of_mass);
}

Here is the call graph for this function:

void RobotisManipulator::addJointActuator	(	Name	actuator_name,
		JointActuator *	joint_actuator,
		std::vector< uint8_t >	id_array,
		const void *	arg
	)

addJointActuator

Parameters

actuator_name
joint_actuator
id_array
arg

Definition at line 109 of file robotis_manipulator.cpp.

{
  joint_actuator_.insert(std::make_pair(actuator_name, joint_actuator));
  if(joint_actuator_.find(actuator_name) != joint_actuator_.end())
  {
    joint_actuator_.at(actuator_name)->init(id_array, arg);
  }
  else
  {
    //error
  }
  for(uint32_t index = 0; index < id_array.size(); index++)
  {
    manipulator_.setComponentActuatorName(manipulator_.findComponentNameUsingId(id_array.at(index)),actuator_name);
  }
  actuator_added_stete_ = true;
}

Here is the call graph for this function:

void RobotisManipulator::addKinematics

(

Kinematics *

kinematics

)

addKinematics

Parameters

kinematics

Definition at line 104 of file robotis_manipulator.cpp.

{
  kinematics_= kinematics;
}

void RobotisManipulator::addTool	(	Name	my_name,
		Name	parent_name,
		Eigen::Vector3d	relative_position,
		Eigen::Matrix3d	relative_orientation,
		int8_t	tool_id = -1,
		double	max_position_limit = M_PI,
		double	min_position_limit = -M_PI,
		double	coefficient = 1.0,
		double	mass = 0.0,
		Eigen::Matrix3d	inertia_tensor = Eigen::Matrix3d::Identity(),
		Eigen::Vector3d	center_of_mass = Eigen::Vector3d::Zero()
	)

addTool

Parameters

my_name
parent_name
relative_position
relative_orientation
tool_id
max_position_limit
min_position_limit
coefficient
mass
inertia_tensor
center_of_mass

Definition at line 81 of file robotis_manipulator.cpp.

{
  manipulator_.addTool(my_name, parent_name, 
                       relative_position, relative_orientation, tool_id, 
                       max_position_limit, min_position_limit, coefficient, mass,
                       inertia_tensor, center_of_mass);
}

Here is the call graph for this function:

void RobotisManipulator::addToolActuator	(	Name	tool_name,
		ToolActuator *	tool_actuator,
		uint8_t	id,
		const void *	arg
	)

addToolActuator

Parameters

tool_name
tool_actuator
id
arg

Definition at line 127 of file robotis_manipulator.cpp.

{
  tool_actuator_.insert(std::make_pair(actuator_name, tool_actuator));
  if(tool_actuator_.find(actuator_name) != tool_actuator_.end())
  {
    tool_actuator_.at(actuator_name)->init(id, arg);
  }
  else
  {
    //error
  }
  manipulator_.setComponentActuatorName(manipulator_.findComponentNameUsingId(id),actuator_name);
  actuator_added_stete_ = true;
}

Here is the call graph for this function:

void RobotisManipulator::addWorld	(	Name	world_name,
		Name	child_name,
		Eigen::Vector3d	world_position = Eigen::Vector3d::Zero(),
		Eigen::Matrix3d	world_orientation = Eigen::Matrix3d::Identity()
	)

addWorld

Parameters

world_name
child_name
world_position
world_orientation

Definition at line 48 of file robotis_manipulator.cpp.

{
  manipulator_.addWorld(world_name, child_name, world_position, world_orientation);
}

Here is the call graph for this function:

bool RobotisManipulator::checkJointLimit	(	Name	component_name,
		double	position
	)

checkJointLimit

Parameters

component_name
position

Returns

Definition at line 868 of file robotis_manipulator.cpp.

{
  if(trajectory_.getManipulator()->checkJointLimit(component_name, joint_position))
    return true;
  else
  {
    log::error("[checkJointLimit] Goal value exceeded limit at " + STRING(component_name) + ".");
    return false;
  }
}

Here is the call graph for this function:

Here is the caller graph for this function:

bool RobotisManipulator::checkJointLimit	(	Name	component_name,
		JointValue	value
	)

checkJointLimit

Parameters

component_name
value

Returns

Definition at line 879 of file robotis_manipulator.cpp.

{
  if(trajectory_.getManipulator()->checkJointLimit(component_name, value.position))
    return true;
  else
  {
    log::error("[checkJointLimit] Goal value exceeded limit at " + STRING(component_name) + ".");
    return false;
  }
}

Here is the call graph for this function:

bool RobotisManipulator::checkJointLimit	(	std::vector< Name >	component_name,
		std::vector< double >	position_vector
	)

checkJointLimit

Parameters

component_name
position_vector

Returns

Definition at line 890 of file robotis_manipulator.cpp.

{
  for(uint32_t index = 0; index < component_name.size(); index++)
  {
    if(!trajectory_.getManipulator()->checkJointLimit(component_name.at(index), position_vector.at(index)))
    {
      log::error("[checkJointLimit] Goal value exceeded limit at " + STRING(component_name.at(index)) + ".");
      return false;
    }
  }
  return true;
}

Here is the call graph for this function:

bool RobotisManipulator::checkJointLimit	(	std::vector< Name >	component_name,
		std::vector< JointValue >	value_vector
	)

checkJointLimit

Parameters

component_name
value_vector

Returns

Definition at line 903 of file robotis_manipulator.cpp.

{
  for(uint32_t index = 0; index < component_name.size(); index++)
  {
    if(!trajectory_.getManipulator()->checkJointLimit(component_name.at(index), value_vector.at(index).position))
    {
      log::error("[checkJointLimit] Goal value exceeded limit at " + STRING(component_name.at(index)) + ".");
      return false;
    }
  }
  return true;
}

Here is the call graph for this function:

void RobotisManipulator::disableActuator

(

Name

actuator_name

)

disableActuator

Parameters

actuator_name

Definition at line 316 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    if(joint_actuator_.find(actuator_name) != joint_actuator_.end())
    {
      joint_actuator_.at(actuator_name)->disable();
    }
    else if(tool_actuator_.find(actuator_name) != tool_actuator_.end())
    {
      tool_actuator_.at(actuator_name)->disable();
    }
    else
    {
      //error
    }
  }
}

void RobotisManipulator::disableAllActuator

(

)

disableAllActuator

Definition at line 403 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    std::map<Name, JointActuator *>::iterator it_joint_actuator;
    for(it_joint_actuator = joint_actuator_.begin(); it_joint_actuator != joint_actuator_.end(); it_joint_actuator++)
    {
      joint_actuator_.at(it_joint_actuator->first)->disable();
    }
    std::map<Name, ToolActuator *>::iterator it_tool_actuator;
    for(it_tool_actuator = tool_actuator_.begin(); it_tool_actuator != tool_actuator_.end(); it_tool_actuator++)
    {
      tool_actuator_.at(it_tool_actuator->first)->disable();
    }
  }
}

void RobotisManipulator::disableAllJointActuator

(

)

disableAllJointActuator

Definition at line 348 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    std::map<Name, JointActuator *>::iterator it_joint_actuator;
    for(it_joint_actuator = joint_actuator_.begin(); it_joint_actuator != joint_actuator_.end(); it_joint_actuator++)
    {
      joint_actuator_.at(it_joint_actuator->first)->disable();
    }
  }
}

void RobotisManipulator::disableAllToolActuator

(

)

disableAllToolActuator

Definition at line 373 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    std::map<Name, ToolActuator *>::iterator it_tool_actuator;
    for(it_tool_actuator = tool_actuator_.begin(); it_tool_actuator != tool_actuator_.end(); it_tool_actuator++)
    {
      tool_actuator_.at(it_tool_actuator->first)->disable();
    }
  }
}

void RobotisManipulator::enableActuator

(

Name

actuator_name

)

enableActuator

Parameters

actuator_name

Definition at line 296 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    if(joint_actuator_.find(actuator_name) != joint_actuator_.end())
    {
      joint_actuator_.at(actuator_name)->enable();
    }
    else if(tool_actuator_.find(actuator_name) != tool_actuator_.end())
    {
      tool_actuator_.at(actuator_name)->enable();
    }
    else
    {
      //error
    }
  }
  trajectory_initialized_state_ = false;
}

void RobotisManipulator::enableAllActuator

(

)

enableAllActuator

Definition at line 385 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    std::map<Name, JointActuator *>::iterator it_joint_actuator;
    for(it_joint_actuator = joint_actuator_.begin(); it_joint_actuator != joint_actuator_.end(); it_joint_actuator++)
    {
      joint_actuator_.at(it_joint_actuator->first)->enable();
    }
    std::map<Name, ToolActuator *>::iterator it_tool_actuator;
    for(it_tool_actuator = tool_actuator_.begin(); it_tool_actuator != tool_actuator_.end(); it_tool_actuator++)
    {
      tool_actuator_.at(it_tool_actuator->first)->enable();
    }
  }
  trajectory_initialized_state_ = false;
}

void RobotisManipulator::enableAllJointActuator

(

)

enableAllJointActuator

Definition at line 335 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    std::map<Name, JointActuator *>::iterator it_joint_actuator;
    for(it_joint_actuator = joint_actuator_.begin(); it_joint_actuator != joint_actuator_.end(); it_joint_actuator++)
    {
      joint_actuator_.at(it_joint_actuator->first)->enable();
    }
  }
  trajectory_initialized_state_ = false;
}

void RobotisManipulator::enableAllToolActuator

(

)

enableAllToolActuator

Definition at line 360 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    std::map<Name, ToolActuator *>::iterator it_tool_actuator;
    for(it_tool_actuator = tool_actuator_.begin(); it_tool_actuator != tool_actuator_.end(); it_tool_actuator++)
    {
      tool_actuator_.at(it_tool_actuator->first)->enable();
    }
  }
  trajectory_initialized_state_ = false;
}

bool RobotisManipulator::getActuatorEnabledState

(

Name

actuator_name

)

getActuatorEnabledState

Parameters

actuator_name

Returns

Definition at line 420 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    if(joint_actuator_.find(actuator_name) != joint_actuator_.end())
    {
      return joint_actuator_.at(actuator_name)->getEnabledState();
    }
    else if(tool_actuator_.find(actuator_name) != tool_actuator_.end())
    {
      return tool_actuator_.at(actuator_name)->getEnabledState();
    }
    else
    {
      return {};
    }
  }
  return {};
}

std::vector< JointValue > RobotisManipulator::getAllActiveJointValue

(

)

getAllActiveJointValue

Returns

Definition at line 171 of file robotis_manipulator.cpp.

{
  return manipulator_.getAllActiveJointValue();
}

Here is the call graph for this function:

std::vector< JointValue > RobotisManipulator::getAllJointValue

(

)

getAllJointValue

Returns

Definition at line 176 of file robotis_manipulator.cpp.

{
  return manipulator_.getAllJointValue();
}

Here is the call graph for this function:

std::vector< double > RobotisManipulator::getAllToolPosition

(

)

getAllToolPosition

Returns

Definition at line 181 of file robotis_manipulator.cpp.

{
  return manipulator_.getAllToolPosition();
}

Here is the call graph for this function:

std::vector< JointValue > RobotisManipulator::getAllToolValue

(

)

getAllToolValue

Returns

Definition at line 186 of file robotis_manipulator.cpp.

{
  return manipulator_.getAllToolValue();
}

Here is the call graph for this function:

DynamicPose RobotisManipulator::getDynamicPose

(

Name

component_name

)

getDynamicPose

Parameters

component_name

Returns

Definition at line 196 of file robotis_manipulator.cpp.

{
  return manipulator_.getComponentDynamicPoseFromWorld(component_name);
}

Here is the call graph for this function:

std::vector< uint8_t > RobotisManipulator::getJointActuatorId

(

Name

actuator_name

)

getJointActuatorId

Parameters

actuator_name

Returns

Definition at line 264 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    if(joint_actuator_.find(actuator_name) != joint_actuator_.end())
    {
      return joint_actuator_.at(actuator_name)->getId();
    }
    else
    {
      //error
    }
  }
  return {};
}

std::vector< JointValue > RobotisManipulator::getJointGoalValueFromTrajectory

(

double

present_time

)

getJointGoalValueFromTrajectory

Parameters

present_time

Returns

Definition at line 1334 of file robotis_manipulator.cpp.

{
  trajectory_.setPresentTime(present_time);

  if(!trajectory_initialized_state_)
  {
    trajectory_.initTrajectoryWaypoint(manipulator_, kinematics_);
    trajectory_initialized_state_ = true;
  }

  if(moving_state_)
  {
    step_moving_state_ = false;
    JointWaypoint joint_goal_way_point;
    double tick_time = trajectory_.getTickTime();
    
    if(tick_time < trajectory_.getMoveTime())
    {
      moving_state_ = true;
      joint_goal_way_point = getTrajectoryJointValue(tick_time);
    }
    else
    {
      moving_state_ = false;
      joint_goal_way_point =  getTrajectoryJointValue(trajectory_.getMoveTime());
    }
    step_moving_state_ = true;
    return joint_goal_way_point;
  }
  return {};
}

Here is the call graph for this function:

std::vector< JointValue > RobotisManipulator::getJointGoalValueFromTrajectoryTickTime

(

double

tick_time

)

getJointGoalValueFromTrajectoryTickTime

Parameters

tick_time

Returns

Definition at line 1366 of file robotis_manipulator.cpp.

{
  if(!trajectory_initialized_state_)
  {
    trajectory_.initTrajectoryWaypoint(manipulator_, kinematics_);
    trajectory_initialized_state_ = true;
  }

  if(moving_state_)
  {
    step_moving_state_ = false;
    JointWaypoint joint_goal_way_point ;
    if(tick_time < trajectory_.getMoveTime())
    {
      moving_state_ = true;
      joint_goal_way_point = getTrajectoryJointValue(tick_time);
    }
    else
    {
      moving_state_ = false;
      joint_goal_way_point = getTrajectoryJointValue(trajectory_.getMoveTime());
    }
    step_moving_state_ = true;
    return joint_goal_way_point;
  }
  return {};
}

Here is the call graph for this function:

JointValue RobotisManipulator::getJointValue

(

Name

joint_name

)

getJointValue

Parameters

joint_name

Returns

Definition at line 161 of file robotis_manipulator.cpp.

{
  return manipulator_.getJointValue(joint_name);
}

Here is the call graph for this function:

KinematicPose RobotisManipulator::getKinematicPose

(

Name

component_name

)

getKinematicPose

Parameters

component_name

Returns

Definition at line 191 of file robotis_manipulator.cpp.

{
  return manipulator_.getComponentKinematicPoseFromWorld(component_name);
}

Here is the call graph for this function:

Manipulator * RobotisManipulator::getManipulator

(

)

getManipulator

Returns

Definition at line 156 of file robotis_manipulator.cpp.

{
  return &manipulator_;
}

bool RobotisManipulator::getMovingState

(

)

getMovingState

Returns

Definition at line 859 of file robotis_manipulator.cpp.

{
  return moving_state_;
}

Here is the caller graph for this function:

Pose RobotisManipulator::getPose

(

Name

component_name

)

getPose

Parameters

component_name

Returns

Definition at line 201 of file robotis_manipulator.cpp.

{
  return manipulator_.getComponentPoseFromWorld(component_name);
}

Here is the call graph for this function:

uint8_t RobotisManipulator::getToolActuatorId

(

Name

actuator_name

)

getToolActuatorId

Parameters

actuator_name

Returns

Definition at line 280 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    if(tool_actuator_.find(actuator_name) != tool_actuator_.end())
    {
      return tool_actuator_.at(actuator_name)->getId();
    }
    else
    {
      //error
    }
  }
  return {};
}

std::vector< JointValue > RobotisManipulator::getToolGoalValue

(

)

getToolGoalValue

Returns

Definition at line 1394 of file robotis_manipulator.cpp.

{
  std::vector<JointValue> result_vector;
  std::vector<Name> tool_component_name = trajectory_.getManipulator()->getAllToolComponentName();
  for(uint32_t index =0; index<tool_component_name.size(); index++)
  {
    result_vector.push_back(trajectory_.getToolGoalValue(tool_component_name.at(index)));
  }
  return result_vector;
}

Here is the call graph for this function:

JointValue RobotisManipulator::getToolValue

(

Name

tool_name

)

getToolValue

Parameters

tool_name

Returns

Definition at line 166 of file robotis_manipulator.cpp.

{
  return manipulator_.getJointValue(tool_name);
}

Here is the call graph for this function:

Trajectory * RobotisManipulator::getTrajectory

(

)

getTrajectory

Returns

Definition at line 920 of file robotis_manipulator.cpp.

{
  return &trajectory_;
}

JointWaypoint RobotisManipulator::getTrajectoryJointValue ( double  tick_time )

private

getTrajectoryJointValue

Parameters

tick_time

Returns

//////////////////////Task Trajectory/////////////////////////

///////////////////Custom Trajectory/////////////////////////

Definition at line 1251 of file robotis_manipulator.cpp.

{
  JointWaypoint joint_way_point_value;

  if(trajectory_.checkTrajectoryType(JOINT_TRAJECTORY))
  {
    joint_way_point_value = trajectory_.getJointTrajectory().getJointWaypoint(tick_time);

    if(!checkJointLimit(trajectory_.getManipulator()->getAllActiveJointComponentName(), joint_way_point_value))
    {
      joint_way_point_value = trajectory_.removeWaypointDynamicData(trajectory_.getPresentJointWaypoint());
      moving_state_ = false;
    }
  }
  else if(trajectory_.checkTrajectoryType(TASK_TRAJECTORY))
  {
    TaskWaypoint task_way_point;
    task_way_point = trajectory_.getTaskTrajectory().getTaskWaypoint(tick_time);

    if(kinematics_->solveInverseKinematics(trajectory_.getManipulator(), trajectory_.getPresentControlToolName(), task_way_point, &joint_way_point_value))
    {
      if(!checkJointLimit(trajectory_.getManipulator()->getAllActiveJointComponentName(), joint_way_point_value))
      {
        joint_way_point_value = trajectory_.removeWaypointDynamicData(trajectory_.getPresentJointWaypoint());
        moving_state_ = false;
      }
    }
    else
    {
      joint_way_point_value = trajectory_.removeWaypointDynamicData(trajectory_.getPresentJointWaypoint());
      log::error("[TASK_TRAJECTORY] fail to solve IK");
      moving_state_ = false;
    }
  }
  else if(trajectory_.checkTrajectoryType(CUSTOM_JOINT_TRAJECTORY))
  {
    joint_way_point_value = trajectory_.getCustomJointTrajectory(trajectory_.getPresentCustomTrajectoryName())->getJointWaypoint(tick_time);

    if(!checkJointLimit(trajectory_.getManipulator()->getAllActiveJointComponentName(), joint_way_point_value))
    {
      joint_way_point_value = trajectory_.removeWaypointDynamicData(trajectory_.getPresentJointWaypoint());
      moving_state_ = false;
    }
  }
  else if(trajectory_.checkTrajectoryType(CUSTOM_TASK_TRAJECTORY))
  {
    TaskWaypoint task_way_point;
    task_way_point = trajectory_.getCustomTaskTrajectory(trajectory_.getPresentCustomTrajectoryName())->getTaskWaypoint(tick_time);

    if(kinematics_->solveInverseKinematics(trajectory_.getManipulator(), trajectory_.getPresentControlToolName(), task_way_point, &joint_way_point_value))
    {
      if(!checkJointLimit(trajectory_.getManipulator()->getAllActiveJointComponentName(), joint_way_point_value))
      {
        joint_way_point_value = trajectory_.removeWaypointDynamicData(trajectory_.getPresentJointWaypoint());
        moving_state_ = false;
      }
    }
    else
    {
      joint_way_point_value = trajectory_.removeWaypointDynamicData(trajectory_.getPresentJointWaypoint());
      log::error("[CUSTOM_TASK_TRAJECTORY] fail to solve IK");
      moving_state_ = false;
    }
  }
  //set present joint task value to trajectory manipulator
  trajectory_.setPresentJointWaypoint(joint_way_point_value);
  trajectory_.updatePresentWaypoint(kinematics_);

//  Eigen::Vector3d print_temp = trajectory_.getManipulator()->getComponentDynamicPoseFromWorld("gripper").angular.velocity;
//  log::PRINT("ang vel");
//  log::PRINT_VECTOR(print_temp);

  return joint_way_point_value;
}

Here is the call graph for this function:

Here is the caller graph for this function:

double RobotisManipulator::getTrajectoryMoveTime

(

)

getTrajectoryMoveTime

Returns

Definition at line 854 of file robotis_manipulator.cpp.

{
  return trajectory_.getMoveTime();
}

Here is the call graph for this function:

Eigen::MatrixXd RobotisManipulator::jacobian

(

Name

tool_name

)

jacobian

Parameters

tool_name

Returns

Definition at line 210 of file robotis_manipulator.cpp.

{
  return kinematics_->jacobian(&manipulator_, tool_name);
}

Here is the call graph for this function:

void RobotisManipulator::makeCustomTrajectory	(	Name	trajectory_name,
		Name	tool_name,
		const void *	arg,
		double	move_time,
		std::vector< JointValue >	present_joint_value = {}
	)

makeCustomTrajectory

Parameters

trajectory_name
tool_name
arg
move_time
present_joint_value

Definition at line 1166 of file robotis_manipulator.cpp.

{
  trajectory_.setTrajectoryType(CUSTOM_TASK_TRAJECTORY);
  trajectory_.setPresentControlToolName(tool_name);
  trajectory_.setMoveTime(move_time);

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

  TaskWaypoint present_task_way_point = trajectory_.getPresentTaskWaypoint(tool_name);

  if(getMovingState())
  {
    moving_state_=false;
    while(!step_moving_state_) ;
  }
  trajectory_.makeCustomTrajectory(trajectory_name, present_task_way_point, arg);
  startMoving();
}

Here is the call graph for this function:

void RobotisManipulator::makeCustomTrajectory	(	Name	trajectory_name,
		const void *	arg,
		double	move_time,
		std::vector< JointValue >	present_joint_value = {}
	)

makeCustomTrajectory

Parameters

trajectory_name
arg
move_time
present_joint_value

Definition at line 1189 of file robotis_manipulator.cpp.

{
  trajectory_.setTrajectoryType(CUSTOM_JOINT_TRAJECTORY);
  trajectory_.setMoveTime(move_time);

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

  JointWaypoint present_joint_way_point = trajectory_.getPresentJointWaypoint();

  if(getMovingState())
  {
    moving_state_=false;
    while(!step_moving_state_) ;
  }
  trajectory_.makeCustomTrajectory(trajectory_name, present_joint_way_point, arg);
  startMoving();
}

Here is the call graph for this function:

void RobotisManipulator::makeJointTrajectory	(	std::vector< double >	goal_joint_position,
		double	move_time,
		std::vector< JointValue >	present_joint_value = {}
	)

makeJointTrajectory

Parameters

goal_joint_position
move_time
present_joint_value

Definition at line 941 of file robotis_manipulator.cpp.

{
  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();
}

Here is the call graph for this function:

Here is the caller graph for this function:

void RobotisManipulator::makeJointTrajectory	(	std::vector< JointValue >	goal_joint_value,
		double	move_time,
		std::vector< JointValue >	present_joint_value = {}
	)

makeJointTrajectory

Parameters

goal_joint_value
move_time
present_joint_value

Definition at line 975 of file robotis_manipulator.cpp.

{
  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();

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

Here is the call graph for this function:

void RobotisManipulator::makeJointTrajectory	(	Name	tool_name,
		Eigen::Vector3d	goal_position,
		double	move_time,
		std::vector< JointValue >	present_joint_value = {}
	)

makeJointTrajectory

Parameters

tool_name
goal_position
move_time
present_joint_value

Definition at line 997 of file robotis_manipulator.cpp.

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

  KinematicPose goal_pose;

  goal_pose.position = goal_position;
  goal_pose.orientation = trajectory_.getManipulator()->getComponentOrientationFromWorld(tool_name);
  makeJointTrajectory(tool_name, goal_pose, move_time);
}

Here is the call graph for this function:

void RobotisManipulator::makeJointTrajectory	(	Name	tool_name,
		Eigen::Matrix3d	goal_orientation,
		double	move_time,
		std::vector< JointValue >	present_joint_value = {}
	)

makeJointTrajectory

Parameters

tool_name
goal_orientation
move_time
present_joint_value

Definition at line 1012 of file robotis_manipulator.cpp.

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

  KinematicPose goal_pose;

  goal_pose.position = trajectory_.getManipulator()->getComponentPositionFromWorld(tool_name);
  goal_pose.orientation = goal_orientation;
  makeJointTrajectory(tool_name, goal_pose, move_time);
}

Here is the call graph for this function:

void RobotisManipulator::makeJointTrajectory	(	Name	tool_name,
		KinematicPose	goal_pose,
		double	move_time,
		std::vector< JointValue >	present_joint_value = {}
	)

makeJointTrajectory

Parameters

tool_name
goal_pose
move_time
present_joint_value

Definition at line 1027 of file robotis_manipulator.cpp.

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

  trajectory_.setTrajectoryType(JOINT_TRAJECTORY);
  trajectory_.setMoveTime(move_time);

  JointWaypoint present_way_point = trajectory_.getPresentJointWaypoint();

  Pose temp_goal_pose;
  temp_goal_pose.kinematic = goal_pose;
  temp_goal_pose = trajectory_.removeWaypointDynamicData(temp_goal_pose);
  std::vector<JointValue> goal_joint_angle;
  if(kinematics_->solveInverseKinematics(trajectory_.getManipulator(), tool_name, temp_goal_pose, &goal_joint_angle))
  {
    if(getMovingState())
    {
      moving_state_=false;
      while(!step_moving_state_) ;
    }
    trajectory_.makeJointTrajectory(present_way_point, goal_joint_angle);
    startMoving();
  }
  else
    log::error("[JOINT_TRAJECTORY] Fail to solve IK");
}

Here is the call graph for this function:

void RobotisManipulator::makeJointTrajectoryFromPresentPosition	(	std::vector< double >	delta_goal_joint_position,
		double	move_time,
		std::vector< JointValue >	present_joint_value = {}
	)

makeJointTrajectoryFromPresentPosition

Parameters

delta_goal_joint_position
move_time
present_joint_value

Definition at line 925 of file robotis_manipulator.cpp.

{
  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);
}

Here is the call graph for this function:

void RobotisManipulator::makeTaskTrajectory	(	Name	tool_name,
		Eigen::Vector3d	goal_position,
		double	move_time,
		std::vector< JointValue >	present_joint_value = {}
	)

makeTaskTrajectory

Parameters

tool_name
goal_position
move_time
present_joint_value

Definition at line 1104 of file robotis_manipulator.cpp.

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

  KinematicPose goal_pose;

  goal_pose.position = goal_position;
  goal_pose.orientation = trajectory_.getManipulator()->getComponentOrientationFromWorld(tool_name);
  makeTaskTrajectory(tool_name, goal_pose, move_time);
}

Here is the call graph for this function:

Here is the caller graph for this function:

void RobotisManipulator::makeTaskTrajectory	(	Name	tool_name,
		Eigen::Matrix3d	goal_orientation,
		double	move_time,
		std::vector< JointValue >	present_joint_value = {}
	)

makeTaskTrajectory

Parameters

tool_name
goal_orientation
move_time
present_joint_value

Definition at line 1119 of file robotis_manipulator.cpp.

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

  KinematicPose goal_pose;

  goal_pose.position = trajectory_.getManipulator()->getComponentPositionFromWorld(tool_name);
  goal_pose.orientation = goal_orientation;
  makeTaskTrajectory(tool_name, goal_pose, move_time);
}

Here is the call graph for this function:

void RobotisManipulator::makeTaskTrajectory	(	Name	tool_name,
		KinematicPose	goal_pose,
		double	move_time,
		std::vector< JointValue >	present_joint_value = {}
	)

makeTaskTrajectory

Parameters

tool_name
goal_pose
move_time
present_joint_value

Definition at line 1134 of file robotis_manipulator.cpp.

{
  trajectory_.setTrajectoryType(TASK_TRAJECTORY);
  trajectory_.setPresentControlToolName(tool_name);
  trajectory_.setMoveTime(move_time);

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

  TaskWaypoint present_task_way_point = trajectory_.getPresentTaskWaypoint(tool_name);

  TaskWaypoint goal_task_way_point;                             //data conversion
  goal_task_way_point.kinematic = goal_pose;
  goal_task_way_point = trajectory_.removeWaypointDynamicData(goal_task_way_point);

  if(getMovingState())
  {
    moving_state_=false;
    while(!step_moving_state_) ;
  }
  trajectory_.makeTaskTrajectory(present_task_way_point, goal_task_way_point);
  startMoving();
}

Here is the call graph for this function:

void RobotisManipulator::makeTaskTrajectoryFromPresentPose	(	Name	tool_name,
		Eigen::Vector3d	position_meter,
		double	move_time,
		std::vector< JointValue >	present_joint_value = {}
	)

makeTaskTrajectoryFromPresentPose

Parameters

tool_name
position_meter
move_time
present_joint_value

Definition at line 1058 of file robotis_manipulator.cpp.

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

  KinematicPose goal_pose;

  goal_pose.position = trajectory_.getManipulator()->getComponentPositionFromWorld(tool_name) + position_meter;
  goal_pose.orientation = trajectory_.getManipulator()->getComponentOrientationFromWorld(tool_name);
  makeTaskTrajectory(tool_name, goal_pose, move_time);
}

Here is the call graph for this function:

void RobotisManipulator::makeTaskTrajectoryFromPresentPose	(	Name	tool_name,
		Eigen::Matrix3d	orientation_meter,
		double	move_time,
		std::vector< JointValue >	present_joint_value = {}
	)

makeTaskTrajectoryFromPresentPose

Parameters

tool_name
orientation_meter
move_time
present_joint_value

Definition at line 1073 of file robotis_manipulator.cpp.

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

  KinematicPose goal_pose;

  goal_pose.position = trajectory_.getManipulator()->getComponentPositionFromWorld(tool_name);
  goal_pose.orientation = orientation_meter * trajectory_.getManipulator()->getComponentOrientationFromWorld(tool_name);
  makeTaskTrajectory(tool_name, goal_pose, move_time);
}

Here is the call graph for this function:

void RobotisManipulator::makeTaskTrajectoryFromPresentPose	(	Name	tool_name,
		KinematicPose	goal_pose_delta,
		double	move_time,
		std::vector< JointValue >	present_joint_value = {}
	)

makeTaskTrajectoryFromPresentPose

Parameters

tool_name
goal_pose_delta
move_time
present_joint_value

Definition at line 1088 of file robotis_manipulator.cpp.

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

  KinematicPose goal_pose;

  goal_pose.position = trajectory_.getManipulator()->getComponentPositionFromWorld(tool_name) + goal_pose_delta.position;
  goal_pose.orientation = goal_pose_delta.orientation * trajectory_.getManipulator()->getComponentOrientationFromWorld(tool_name);
  makeTaskTrajectory(tool_name, goal_pose, move_time);
}

Here is the call graph for this function:

void RobotisManipulator::makeToolTrajectory	(	Name	tool_name,
		double	tool_goal_position
	)

makeToolTrajectory

Parameters

tool_name
tool_goal_position

Definition at line 1235 of file robotis_manipulator.cpp.

{
  JointValue tool_value;
  tool_value.position = tool_goal_position;
  tool_value.velocity = 0.0;
  tool_value.acceleration = 0.0;
  tool_value.effort =0.0;

  if(checkJointLimit(tool_name, tool_value))
  {
    trajectory_.setToolGoalValue(tool_name, tool_value);
  }
}

Here is the call graph for this function:

void RobotisManipulator::printManipulatorSetting

(

)

printManipulatorSetting

Definition at line 99 of file robotis_manipulator.cpp.

{
  manipulator_.printManipulatorSetting();
}

Here is the call graph for this function:

std::vector< JointValue > RobotisManipulator::receiveAllJointActuatorValue

(

)

receiveAllJointActuatorValue

Returns

Definition at line 645 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    std::vector<JointValue> get_value_vector;
    std::vector<uint8_t> get_actuator_id;

    std::vector<JointValue> single_value_vector;
    std::vector<uint8_t> single_actuator_id;
    std::map<Name, JointActuator *>::iterator it_joint_actuator;
    for(it_joint_actuator = joint_actuator_.begin(); it_joint_actuator != joint_actuator_.end(); it_joint_actuator++)
    {
      single_actuator_id = joint_actuator_.at(it_joint_actuator->first)->getId();
      single_value_vector = joint_actuator_.at(it_joint_actuator->first)->receiveJointActuatorValue(single_actuator_id);
      for(uint32_t index=0; index < single_actuator_id.size(); index++)
      {
        get_actuator_id.push_back(single_actuator_id.at(index));
        get_value_vector.push_back(single_value_vector.at(index));
      }
    }

    std::map<Name, Component>::iterator it;
    std::vector<JointValue> result_vector;
    JointValue result;

    for (it = manipulator_.getIteratorBegin(); it != manipulator_.getIteratorEnd(); it++)
    {
      for(uint32_t index2 = 0; index2 < get_actuator_id.size(); index2++)
      {
        if(manipulator_.checkComponentType(it->first,ACTIVE_JOINT_COMPONENT) && manipulator_.getId(it->first) == get_actuator_id.at(index2))
        {
          double coefficient = manipulator_.getCoefficient(it->first);
          result.position = get_value_vector.at(index2).position * coefficient;
          result.velocity = get_value_vector.at(index2).velocity * coefficient;
          result.acceleration = get_value_vector.at(index2).acceleration * coefficient;
          result.effort = get_value_vector.at(index2).effort;
          manipulator_.setJointValue(it->first, result);
          result_vector.push_back(result);
        }
      }
    }

    return result_vector;
  }
  return {};
}

Here is the call graph for this function:

std::vector< JointValue > RobotisManipulator::receiveAllToolActuatorValue

(

)

receiveAllToolActuatorValue

Returns

Definition at line 818 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    std::vector<Name> tool_component_name;
    tool_component_name = manipulator_.getAllToolComponentName();
    std::vector<JointValue> result_vector;
    ActuatorValue result;
    for (uint32_t index = 0; index < tool_component_name.size(); index++)
    {
      result = tool_actuator_.at(manipulator_.getComponentActuatorName(tool_component_name.at(index)))->receiveToolActuatorValue();

      double coefficient = manipulator_.getCoefficient(tool_component_name.at(index));
      result.position = result.position * coefficient;
      result.velocity = result.velocity * coefficient;
      result.acceleration = result.acceleration * coefficient;

      manipulator_.setJointValue(tool_component_name.at(index), result);
      result_vector.push_back(result);
    }
    return result_vector;
  }
  return {};
}

Here is the call graph for this function:

JointValue RobotisManipulator::receiveJointActuatorValue

(

Name

joint_component_name

)

receiveJointActuatorValue

Parameters

joint_component_name

Returns

Definition at line 576 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    std::vector<uint8_t> actuator_id;
    std::vector<JointValue> result;

    actuator_id.push_back(manipulator_.getId(joint_component_name));

    result = joint_actuator_.at(manipulator_.getComponentActuatorName(joint_component_name))->receiveJointActuatorValue(actuator_id);

    double coefficient = manipulator_.getCoefficient(joint_component_name);
    result.at(0).position = result.at(0).position * coefficient;
    result.at(0).velocity = result.at(0).velocity * coefficient;
    result.at(0).acceleration = result.at(0).acceleration * coefficient;
    result.at(0).effort = result.at(0).effort;

    manipulator_.setJointValue(joint_component_name, result.at(0));
    return result.at(0);
  }
  return {};
}

Here is the call graph for this function:

std::vector< JointValue > RobotisManipulator::receiveMultipleJointActuatorValue

(

std::vector< Name >

joint_component_name

)

receiveMultipleJointActuatorValue

Parameters

joint_component_name

Returns

Definition at line 599 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    std::vector<JointValue> get_value_vector;
    std::vector<uint8_t> get_actuator_id;

    std::vector<JointValue> single_value_vector;
    std::vector<uint8_t> single_actuator_id;
    std::map<Name, JointActuator *>::iterator it_joint_actuator;
    for(it_joint_actuator = joint_actuator_.begin(); it_joint_actuator != joint_actuator_.end(); it_joint_actuator++)
    {
      single_actuator_id = joint_actuator_.at(it_joint_actuator->first)->getId();
      single_value_vector = joint_actuator_.at(it_joint_actuator->first)->receiveJointActuatorValue(single_actuator_id);
      for(uint32_t index=0; index < single_actuator_id.size(); index++)
      {
        get_actuator_id.push_back(single_actuator_id.at(index));
        get_value_vector.push_back(single_value_vector.at(index));
      }
    }

    std::vector<JointValue> result_vector;
    JointValue result;

    for(uint32_t index = 0; index < joint_component_name.size(); index++)
    {
      for(uint32_t index2 = 0; index2 < get_actuator_id.size(); index2++)
      {
        if(manipulator_.getId(joint_component_name.at(index)) == get_actuator_id.at(index2))
        {
          double coefficient = manipulator_.getCoefficient(joint_component_name.at(index));
          result.position = get_value_vector.at(index2).position * coefficient;
          result.velocity = get_value_vector.at(index2).velocity * coefficient;
          result.acceleration = get_value_vector.at(index2).acceleration * coefficient;
          result.effort = get_value_vector.at(index2).effort;
          manipulator_.setJointValue(joint_component_name.at(index), result);
          result_vector.push_back(result);
        }
      }
    }

    return result_vector;
  }
  return {};
}

Here is the call graph for this function:

std::vector< JointValue > RobotisManipulator::receiveMultipleToolActuatorValue

(

std::vector< Name >

tool_component_name

)

receiveMultipleToolActuatorValue

Parameters

tool_component_name

Returns

Definition at line 795 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    std::vector<JointValue> result_vector;
    ActuatorValue result;
    for (uint32_t index = 0; index < tool_component_name.size(); index++)
    {
      result = tool_actuator_.at(manipulator_.getComponentActuatorName(tool_component_name.at(index)))->receiveToolActuatorValue();

      double coefficient = manipulator_.getCoefficient(tool_component_name.at(index));
      result.position = result.position * coefficient;
      result.velocity = result.velocity * coefficient;
      result.acceleration = result.acceleration * coefficient;

      manipulator_.setJointValue(tool_component_name.at(index), result);
      result_vector.push_back(result);
    }
    return result_vector;
  }
  return {};
}

Here is the call graph for this function:

JointValue RobotisManipulator::receiveToolActuatorValue

(

Name

tool_component_name

)

receiveToolActuatorValue

Parameters

tool_component_name

Returns

Definition at line 777 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    JointValue result;
    result = tool_actuator_.at(manipulator_.getComponentActuatorName(tool_component_name))->receiveToolActuatorValue();

    double coefficient = manipulator_.getCoefficient(tool_component_name);
    result.position = result.position * coefficient;
    result.velocity = result.velocity * coefficient;
    result.acceleration = result.acceleration * coefficient;

    manipulator_.setJointValue(tool_component_name, result);
    return result;
  }
  return {};
}

Here is the call graph for this function:

bool RobotisManipulator::sendAllJointActuatorValue

(

std::vector< JointValue >

value_vector

)

sendAllJointActuatorValue

Parameters

value_vector

Returns

Definition at line 523 of file robotis_manipulator.cpp.

{
  // trajectory manipulator set
  // trajectory_.setPresentJointWayPoint(value_vector);
  // trajectory_.updatePresentWayPoint(kinematics_dynamics_);

  if(actuator_added_stete_)
  {
    std::map<Name, Component>::iterator it;
    std::vector<int8_t> joint_id;
    int index = 0;
    for (it = manipulator_.getIteratorBegin(); it != manipulator_.getIteratorEnd(); it++)
    {
      if(manipulator_.checkComponentType(it->first, ACTIVE_JOINT_COMPONENT))
      {
        double coefficient = manipulator_.getCoefficient(it->first);
        value_vector.at(index).position = value_vector.at(index).position / coefficient;
        value_vector.at(index).velocity = value_vector.at(index).velocity / coefficient;
        value_vector.at(index).acceleration = value_vector.at(index).acceleration / coefficient;
        value_vector.at(index).effort = value_vector.at(index).effort;
        joint_id.push_back(manipulator_.getId(it->first));
        index++;
      }
    }

    std::vector<uint8_t> single_actuator_id;
    std::vector<JointValue> single_value_vector;
    std::map<Name, JointActuator *>::iterator it_joint_actuator;
    for(it_joint_actuator = joint_actuator_.begin(); it_joint_actuator != joint_actuator_.end(); it_joint_actuator++)
    {
      single_actuator_id = joint_actuator_.at(it_joint_actuator->first)->getId();
      for(uint32_t index = 0; index < single_actuator_id.size(); index++)
      {
        for(uint32_t index2=0; index2 < joint_id.size(); index2++)
        {
           if(single_actuator_id.at(index) == joint_id.at(index2))
           {
             single_value_vector.push_back(value_vector.at(index2));
           }
        }
      }
      joint_actuator_.at(it_joint_actuator->first)->sendJointActuatorValue(single_actuator_id, single_value_vector);
    }
    return true;
  }
  else
  {
    //set to manipulator
    manipulator_.setAllActiveJointValue(value_vector);
  }
  return false;
}

Here is the call graph for this function:

bool RobotisManipulator::sendAllToolActuatorValue

(

std::vector< JointValue >

value_vector

)

sendAllToolActuatorValue

Parameters

value_vector

Returns

Definition at line 748 of file robotis_manipulator.cpp.

{
  // trajectory manipulator set
  // trajectory_.getManipulator()->setAllToolValue(value_vector);

  if(actuator_added_stete_)
  {
    std::vector<Name> tool_component_name;
    tool_component_name = manipulator_.getAllToolComponentName();
    for (uint32_t index = 0; index < tool_component_name.size(); index++)
    {
      double coefficient = manipulator_.getCoefficient(tool_component_name.at(index));
      value_vector.at(index).position = value_vector.at(index).position / coefficient;
      value_vector.at(index).velocity = value_vector.at(index).velocity / coefficient;
      value_vector.at(index).acceleration = value_vector.at(index).acceleration / coefficient;

      if(!tool_actuator_.at(manipulator_.getComponentActuatorName(tool_component_name.at(index)))->sendToolActuatorValue(value_vector.at(index)))
        return false;
    }
    return true;
  }
  else
  {
    //set to manipualtor
    manipulator_.setAllToolValue(value_vector);
  }
  return false;
}

Here is the call graph for this function:

bool RobotisManipulator::sendJointActuatorValue	(	Name	joint_component_name,
		JointValue	value
	)

sendJointActuatorValue

Parameters

joint_component_name
value

Returns

Definition at line 440 of file robotis_manipulator.cpp.

{
  // 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;
}

Here is the call graph for this function:

bool RobotisManipulator::sendMultipleJointActuatorValue	(	std::vector< Name >	joint_component_name,
		std::vector< JointValue >	value_vector
	)

sendMultipleJointActuatorValue

Parameters

joint_component_name
value_vector

Returns

Definition at line 470 of file robotis_manipulator.cpp.

{
  if(joint_component_name.size() != value_vector.size())
    return false; //error;

  // trajectory manipulator set
  // for(uint8_t index = 0; index < joint_component_name.size(); index++)
  //   trajectory_.getManipulator()->setJointValue(joint_component_name.at(index), value_vector.at(index));
  // trajectory_.updatePresentWayPoint(kinematics_dynamics_);

  if(actuator_added_stete_)
  {
    std::vector<int8_t> joint_id;
    for(uint32_t index = 0; index < value_vector.size(); index++)
    {
      double coefficient = manipulator_.getCoefficient(joint_component_name.at(index));
      value_vector.at(index).position = value_vector.at(index).position / coefficient;
      value_vector.at(index).velocity = value_vector.at(index).velocity / coefficient;
      value_vector.at(index).acceleration = value_vector.at(index).acceleration / coefficient;
      value_vector.at(index).effort = value_vector.at(index).effort;
      joint_id.push_back(manipulator_.getId(joint_component_name.at(index)));
    }

    std::vector<uint8_t> single_actuator_id;
    std::vector<JointValue> single_value_vector;
    std::map<Name, JointActuator *>::iterator it_joint_actuator;
    for(it_joint_actuator = joint_actuator_.begin(); it_joint_actuator != joint_actuator_.end(); it_joint_actuator++)
    {
      single_actuator_id = joint_actuator_.at(it_joint_actuator->first)->getId();
      for(uint32_t index = 0; index < single_actuator_id.size(); index++)
      {
        for(uint32_t index2=0; index2 < joint_id.size(); index2++)
        {
           if(single_actuator_id.at(index) == joint_id.at(index2))
           {
             single_value_vector.push_back(value_vector.at(index2));
           }
        }
      }
      joint_actuator_.at(it_joint_actuator->first)->sendJointActuatorValue(single_actuator_id, single_value_vector);
    }
    return true;
  }
  else
  {
    //set to manipulator
    for(uint8_t index = 0; index < joint_component_name.size(); index++)
      manipulator_.setJointValue(joint_component_name.at(index), value_vector.at(index));
    return true;
  }
  return false;
}

Here is the call graph for this function:

bool RobotisManipulator::sendMultipleToolActuatorValue	(	std::vector< Name >	tool_component_name,
		std::vector< JointValue >	value_vector
	)

sendMultipleToolActuatorValue

Parameters

tool_component_name
value_vector

Returns

Definition at line 718 of file robotis_manipulator.cpp.

{
  // trajectory manipulator set
  // for(uint8_t index = 0; index < tool_component_name.size(); index++)
  //   trajectory_.getManipulator()->setJointValue(tool_component_name.at(index), value_vector.at(index));

  if(actuator_added_stete_)
  {
    for (uint32_t index = 0; index < tool_component_name.size(); index++)
    {
      double coefficient = manipulator_.getCoefficient(tool_component_name.at(index));
      value_vector.at(index).position = value_vector.at(index).position / coefficient;
      value_vector.at(index).velocity = value_vector.at(index).velocity / coefficient;
      value_vector.at(index).acceleration = value_vector.at(index).acceleration / coefficient;

      if(!tool_actuator_.at(manipulator_.getComponentActuatorName(tool_component_name.at(index)))->sendToolActuatorValue(value_vector.at(index)))
        return false;
    }
    return true;
  }
  else
  {
    //set to manipulator
    for(uint8_t index = 0; index < tool_component_name.size(); index++)
      manipulator_.setJointValue(tool_component_name.at(index), value_vector.at(index));
    return true;
  }
  return false;
}

Here is the call graph for this function:

bool RobotisManipulator::sendToolActuatorValue	(	Name	tool_component_name,
		JointValue	value
	)

sendToolActuatorValue

Parameters

tool_component_name
value

Returns

Definition at line 693 of file robotis_manipulator.cpp.

{
  // trajectory manipulator set
  // trajectory_.getManipulator()->setJointValue(tool_component_name,value);

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

    return tool_actuator_.at(manipulator_.getComponentActuatorName(tool_component_name))->sendToolActuatorValue(value);
  }
  else
  {
    //set to manipulator
    manipulator_.setJointValue(tool_component_name, value);
    return true;
  }
  return false;
}

Here is the call graph for this function:

void RobotisManipulator::setCustomTrajectoryOption	(	Name	trajectory_name,
		const void *	arg
	)

setCustomTrajectoryOption

Parameters

trajectory_name
arg

Definition at line 1161 of file robotis_manipulator.cpp.

{
  trajectory_.setCustomTrajectoryOption(trajectory_name, arg);
}

Here is the call graph for this function:

void RobotisManipulator::setJointActuatorMode	(	Name	actuator_name,
		std::vector< uint8_t >	id_array,
		const void *	arg
	)

setJointActuatorMode

Parameters

actuator_name
id_array
arg

Definition at line 234 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    if(joint_actuator_.find(actuator_name) != joint_actuator_.end())
    {
      joint_actuator_.at(actuator_name)->setMode(id_array, arg);
    }
    else
    {
      robotis_manipulator::log::error("[jointActuatorSetMode] Worng Actuator Name.");
    }
  }
}

Here is the call graph for this function:

void RobotisManipulator::setKinematicsOption

(

const void *

arg

)

setKinematicsOption

Parameters

arg

Definition at line 225 of file robotis_manipulator.cpp.

{
  kinematics_->setOption(arg);
}

Here is the call graph for this function:

void RobotisManipulator::setToolActuatorMode	(	Name	actuator_name,
		const void *	arg
	)

setToolActuatorMode

Parameters

actuator_name
arg

Definition at line 249 of file robotis_manipulator.cpp.

{
  if(actuator_added_stete_)
  {
    if(tool_actuator_.find(actuator_name) != tool_actuator_.end())
    {
      tool_actuator_.at(actuator_name)->setMode(arg);
    }
    else
    {
      //error
    }
  }
}

void RobotisManipulator::sleepTrajectory	(	double	wait_time,
		std::vector< JointValue >	present_joint_value = {}
	)

sleepTrajectory

Parameters

wait_time
present_joint_value

Definition at line 1211 of file robotis_manipulator.cpp.

{
  trajectory_.setTrajectoryType(JOINT_TRAJECTORY);
  trajectory_.setMoveTime(wait_time);

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

  JointWaypoint present_joint_way_point = trajectory_.getPresentJointWaypoint();
  JointWaypoint goal_way_point_vector = trajectory_.getPresentJointWaypoint();
  goal_way_point_vector = trajectory_.removeWaypointDynamicData(goal_way_point_vector);

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

Here is the call graph for this function:

void RobotisManipulator::solveForwardKinematics

(

)

solveForwardKinematics

Definition at line 215 of file robotis_manipulator.cpp.

{
  return kinematics_->solveForwardKinematics(&manipulator_);
}

Here is the call graph for this function:

bool RobotisManipulator::solveInverseKinematics	(	Name	tool_name,
		Pose	goal_pose,
		std::vector< JointValue > *	goal_joint_value
	)

solveInverseKinematics

Parameters

tool_name
goal_pose
goal_joint_value

Returns

Definition at line 220 of file robotis_manipulator.cpp.

{
  return kinematics_->solveInverseKinematics(&manipulator_, tool_name, goal_pose, goal_joint_value);
}

Here is the call graph for this function:

void RobotisManipulator::startMoving ( )

private

startMoving

Definition at line 848 of file robotis_manipulator.cpp.

{
  moving_state_ = true;
  trajectory_.setStartTimeToPresentTime();
}

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

bool robotis_manipulator::RobotisManipulator::actuator_added_stete_

private

Definition at line 54 of file robotis_manipulator.h.

std::map<Name, JointActuator *> robotis_manipulator::RobotisManipulator::joint_actuator_

private

Definition at line 50 of file robotis_manipulator.h.

Kinematics* robotis_manipulator::RobotisManipulator::kinematics_

private

Definition at line 49 of file robotis_manipulator.h.

Manipulator robotis_manipulator::RobotisManipulator::manipulator_

private

Definition at line 47 of file robotis_manipulator.h.

bool robotis_manipulator::RobotisManipulator::moving_state_

private

Definition at line 55 of file robotis_manipulator.h.

bool robotis_manipulator::RobotisManipulator::step_moving_state_

private

Definition at line 56 of file robotis_manipulator.h.

std::map<Name, ToolActuator *> robotis_manipulator::RobotisManipulator::tool_actuator_

private

Definition at line 51 of file robotis_manipulator.h.

Trajectory robotis_manipulator::RobotisManipulator::trajectory_

private

Definition at line 48 of file robotis_manipulator.h.

bool robotis_manipulator::RobotisManipulator::trajectory_initialized_state_

private

Definition at line 53 of file robotis_manipulator.h.

---

The documentation for this class was generated from the following files:

• include/robotis_manipulator/robotis_manipulator.h
• src/robotis_manipulator/robotis_manipulator.cpp