java怎么让机械臂动起来_Kinova_MOVO机器人基本操作及机械臂倒水demo实现

// movo_moveit_uoperbody.cpp

#include

#include

#include

#include

#include

#include

#include

//grasp

#include

#include

#include

#include

int main(int argc, char **argv)

{

ros::init(argc, argv, "movo_moveit");

ros::NodeHandle node_handle;

ros::AsyncSpinner spinner(1);

spinner.start();

//action grasp

actionlib::SimpleActionClient<:grippercommandaction> acr("/movo/right_gripper_controller/gripper_cmd", true);

ROS_INFO("Waiting for the gripper action server");

acr.waitForServer(ros::Duration(3));

ROS_INFO("Connected to move base server");

control_msgs::GripperCommandGoal grigoal_right;

actionlib::SimpleActionClient<:grippercommandaction> acl("/movo/left_gripper_controller/gripper_cmd", true);

// Wait 60 seconds for the action server to become available

ROS_INFO("Waiting for the gripper action server");

acl.waitForServer(ros::Duration(3));

ROS_INFO("Connected to move base server");

control_msgs::GripperCommandGoal grigoal_left;

moveit::planning_interface::MoveGroup group("upper_body");

moveit::planning_interface::MoveGroup l_group("left_arm");

moveit::planning_interface::MoveGroup r_group("right_arm");

group.setNamedTarget("homed_2");

moveit::planning_interface::MoveGroup::Plan upperbody_plan;

bool success_upper = group.plan(upperbody_plan);

ROS_INFO("Visualizing plan (pose goal) %s",success_upper?"":"FAILED");

if(success_upper)

group.execute(upperbody_plan);

group.setNamedTarget("plan_grasp");

moveit::planning_interface::MoveGroup::Plan upperbody_plan_1;

bool success_upper_1 = group.plan(upperbody_plan_1);

ROS_INFO("Visualizing plan (pose goal) %s",success_upper?"":"FAILED");

if(success_upper_1)

{

group.execute(upperbody_plan_1);

}

//opengrasp

grigoal_right.command.position = 0.16;

ROS_INFO("Sending goal");

acr.sendGoal(grigoal_right);

acr.waitForResult();

if (acr.get