OpenMANIPULATOR-X gazebo simulation 관련 질문드립니다

Community 한국어 커뮤니티
,
1

안녕하세요. Ubuntu 20.04 사용 중이며, OpenMANIPULATOR-X로 관절 각도만 조절하고자 합니다. 특정 관절의 위치들을 계속 반복하는 프로젝트를 생각 중입니다.
기존 소스인 open_manipulator_teleop의 open_manipulator_teleop_keboard.launch를 변형해 아래의 C++ 코드를 작성했습니다.

C++코드

#include “test/control.h”
#include <math.h>
#include <stdlib.h>
#include

OpenManipulatorTeleop::OpenManipulatorTeleop()
: node_handle_(“”),
priv_node_handle_(“~”)
{
/************************************************************
** Initialize variables
************************************************************/
present_joint_angle_.resize(NUM_OF_JOINT);
present_kinematic_position_.resize(3);

/************************************************************
** Initialize ROS Subscribers and Clients
************************************************************/
initSubscriber();
initClient();

disableWaitingForEnter();
}

OpenManipulatorTeleop::~OpenManipulatorTeleop()
{
restoreTerminalSettings();
ros::shutdown();
}

void OpenManipulatorTeleop::initClient()
{
goal_joint_space_path_client_ = node_handle_.serviceClient<open_manipulator_msgs::SetJointPosition>(“goal_joint_space_path”);
goal_joint_space_path_from_present_client_ = node_handle_.serviceClient<open_manipulator_msgs::SetJointPosition>(“goal_joint_space_path_from_present”);
goal_task_space_path_from_present_position_only_client_ = node_handle_.serviceClient<open_manipulator_msgs::SetKinematicsPose>(“goal_task_space_path_from_present_position_only”);
goal_tool_control_client_ = node_handle_.serviceClient<open_manipulator_msgs::SetJointPosition>(“goal_tool_control”);
}

void OpenManipulatorTeleop::initSubscriber()
{
joint_states_sub_ = node_handle_.subscribe(“joint_states”, 10, &OpenManipulatorTeleop::jointStatesCallback, this);
kinematics_pose_sub_ = node_handle_.subscribe(“kinematics_pose”, 10, &OpenManipulatorTeleop::kinematicsPoseCallback, this);
}

void OpenManipulatorTeleop::jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
{
std::vector temp_angle;
temp_angle.resize(NUM_OF_JOINT);
for (std::vector::size_type i = 0; i < msg->name.size(); i ++)
{
if (!msg->name.at(i).compare(“joint1”)) temp_angle.at(0) = (msg->position.at(i));
else if (!msg->name.at(i).compare(“joint2”)) temp_angle.at(1) = (msg->position.at(i));
else if (!msg->name.at(i).compare(“joint3”)) temp_angle.at(2) = (msg->position.at(i));
else if (!msg->name.at(i).compare(“joint4”)) temp_angle.at(3) = (msg->position.at(i));
}
present_joint_angle_ = temp_angle;
}

void OpenManipulatorTeleop::kinematicsPoseCallback(const open_manipulator_msgs::KinematicsPose::ConstPtr &msg)
{
std::vector temp_position;
temp_position.push_back(msg->pose.position.x);
temp_position.push_back(msg->pose.position.y);
temp_position.push_back(msg->pose.position.z);
present_kinematic_position_ = temp_position;
}

std::vector OpenManipulatorTeleop::getPresentJointAngle()
{
return present_joint_angle_;
}

std::vector OpenManipulatorTeleop::getPresentKinematicsPose()
{
return present_kinematic_position_;
}

bool OpenManipulatorTeleop::setJointSpacePathFromPresent(std::vectorstd::string joint_name, std::vector joint_angle, double path_time)
{
open_manipulator_msgs::SetJointPosition srv;
srv.request.joint_position.joint_name = joint_name;
srv.request.joint_position.position = joint_angle;
srv.request.path_time = path_time;

if (goal_joint_space_path_from_present_client_.call(srv))
{
return srv.response.is_planned;
}
return false;
}

bool OpenManipulatorTeleop::setJointSpacePath(std::vectorstd::string joint_name, std::vector joint_angle, double path_time)
{
open_manipulator_msgs::SetJointPosition srv;
srv.request.joint_position.joint_name = joint_name;
srv.request.joint_position.position = joint_angle;
srv.request.path_time = path_time;

if (goal_joint_space_path_client_.call(srv))
{
return srv.response.is_planned;
}
return false;
}

bool OpenManipulatorTeleop::setToolControl(std::vector joint_angle)
{
open_manipulator_msgs::SetJointPosition srv;
srv.request.joint_position.joint_name.push_back(priv_node_handle_.paramstd::string(“end_effector_name”, “gripper”));
srv.request.joint_position.position = joint_angle;

if (goal_tool_control_client_.call(srv))
{
return srv.response.is_planned;
}
return false;
}

bool OpenManipulatorTeleop::setTaskSpacePathFromPresentPositionOnly(std::vector kinematics_pose, double path_time)
{
open_manipulator_msgs::SetKinematicsPose srv;
srv.request.planning_group = priv_node_handle_.paramstd::string(“end_effector_name”, “gripper”);
srv.request.kinematics_pose.pose.position.x = kinematics_pose.at(0);
srv.request.kinematics_pose.pose.position.y = kinematics_pose.at(1);
srv.request.kinematics_pose.pose.position.z = kinematics_pose.at(2);
srv.request.path_time = path_time;

if (goal_task_space_path_from_present_position_only_client_.call(srv))
{
return srv.response.is_planned;
}
return false;
}

void OpenManipulatorTeleop::printText()
{

printf(“q to quit\n”);
printf(“---------------------------\n”);
printf(“Present Joint Angle J1: %.3lf J2: %.3lf J3: %.3lf J4: %.3lf\n”,
getPresentJointAngle().at(0),
getPresentJointAngle().at(1),
getPresentJointAngle().at(2),
getPresentJointAngle().at(3));
printf(“Present Kinematics Position X: %.3lf Y: %.3lf Z: %.3lf\n”,
getPresentKinematicsPose().at(0),
getPresentKinematicsPose().at(1),
getPresentKinematicsPose().at(2));
printf(“---------------------------\n”);

}

void OpenManipulatorTeleop::setGoal(void)
{

std::vector<std::string> joint_name;
std::vector<double> joint_angle;
double path_time = 2.0;

joint_name.push_back("joint1"); joint_angle.push_back(0.0);
joint_name.push_back("joint2"); joint_angle.push_back(0.0);
joint_name.push_back("joint3"); joint_angle.push_back(0.0);
joint_name.push_back("joint4"); joint_angle.push_back(0.0);
setJointSpacePath(joint_name, joint_angle, path_time);

char ch;

while (ros::ok() && (ch = std::getchar()) != 'q')
{
  ros::spinOnce();
  joint_name.push_back("joint1"); joint_angle.push_back(0.0);
  joint_name.push_back("joint2"); joint_angle.push_back(0.0);
  joint_name.push_back("joint3"); joint_angle.push_back(0.0);
  joint_name.push_back("joint4"); joint_angle.push_back(0.5);
  setJointSpacePath(joint_name, joint_angle, path_time);

  ros::spinOnce();

  joint_name.push_back("joint1"); joint_angle.push_back(0.0);
  joint_name.push_back("joint2"); joint_angle.push_back(0.0);
  joint_name.push_back("joint3"); joint_angle.push_back(0.0);
  joint_name.push_back("joint4"); joint_angle.push_back(1.0);
  setJointSpacePath(joint_name, joint_angle, path_time);
}

}

void OpenManipulatorTeleop::restoreTerminalSettings(void)
{
tcsetattr(0, TCSANOW, &oldt_); /* Apply saved settings */
}

void OpenManipulatorTeleop::disableWaitingForEnter(void)
{
struct termios newt;

tcgetattr(0, &oldt_); /* Save terminal settings /
newt = oldt_; / Init new settings /
newt.c_lflag &= ~(ICANON | ECHO); / Change settings /
tcsetattr(0, TCSANOW, &newt); / Apply settings */
}

int main(int argc, char **argv)
{
// Init ROS node
ros::init(argc, argv, “control”);
OpenManipulatorTeleop openManipulatorTeleop;

openManipulatorTeleop.printText();
ros::spinOnce();
openManipulatorTeleop.setGoal();

return 0;
}

코드를 작성한 뒤 런치 파일을 만들고,
roslaunch open_manipulator_gazebo open_manipulator_gazebo.launch 및
roslaunch open_manipulator_controller open_manipulator_controller.launch use_platform:=false를 사용해 gazebo에서 시뮬레이션해보았으나 관절이 움직이지 않았습니다.
어떻게 해야 할까요?

감사합니다.