Kappa95's profile - activity

Hi community, I'm a newbie of ROS and i had started to work with a Yumi robot of ABB (IRB 14000). In order to use ROS Kinetic (with ubuntu 16.04) with this robot i followed these guides and installed these packages Now i'm in some trouble with the trajectory planning mainly because:

1. The trajectory planning takes a lot of time also for very easy poses: for example
2. The planners do "whatever they want" (but probably i fixed with my old question link text)
3. Sometime the planner does not compute the path for no reason

I searched a lot in the web and i had setted/changed different parameters in the ompl_planning.yaml file. For example now i'm using:

  RRTstarkConfigDefault:
type: geometric::RRTstar
termination_condition: Iteration[10]
range: 0.0  # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
goal_bias: 0.05  # When close to goal select goal, with this probability? default: 0.05
delay_collision_checking: 1  # Stop collision checking as soon as C-free parent found. default 1

Is there are also a "good way" to program the motion? I followed all the tutorials regarding Python language because is the one that i'm using and the code written by me is:

#!/usr/bin/env python


import copy
from typing import Any
from moveit_msgs.msg import *
from moveit_commander import *
from std_msgs.msg import String
import geometry_msgs.msg
from yumi_utils import PI, gripper_effort
from yumi_hw.srv import *
from tf.transformations import quaternion_from_euler, concatenate_matrices, euler_matrix, quaternion_from_matrix


# Arm IDs
RIGHT = 1  # :ID of the right arm
LEFT = 2  # :ID of the left arm
BOTH = 3  # :ID of both_arms

# Defining the measure of the table
table_height = 0.025  # [m] :The height of the upper surface of the table (z)
table_length = 1.200  # [m] :The width of the table (y)
table_width = 0.400  # [m] :The width of the table (x)

# Length of the Gripper from datasheet
z_gripper = 0.136  # [m]

# Distance of the center of the cam from yumi_link_7_r
z_cam = 0.040  # [m]

# Length of the test tube
length_tube = 0.125  # [m]

# Choice of the planners
planner = "RRTstarkConfigDefault"  # Asymptotic optimal tree-based planner
# planner = "ESTkConfigDefault"  # Default: tree-based planner
# planner = "RRTConnectConfigDefault"  # Tree-based planner
# planner = "PRMstarkConfigDefault"  # Probabilistic Roadmap planner

planning_attempts = 100  # planning attempts
planning_time = 50  # [s] Planning time for computation

# Defining the workspace [min X, min Y, min Z, max X, max Y, max Z]
ws_R = [0.000, -table_length/2, table_height, 0.600, 0.200, 0.593]
ws_L = [0.000, -0.200, table_height, 0.600, table_length, 0.593]

# Initialization of the Node
rospy.init_node('demo_test', anonymous=True, log_level=rospy.DEBUG)
# Initialization of Moveit
rospy.loginfo('Starting the Initialization')
roscpp_initialize(sys.argv)

robot = RobotCommander()
scene = PlanningSceneInterface()
mpr = MotionPlanRequest()
rospy.sleep(1.0)

# Left arm
group_l = MoveGroupCommander("left_arm")
# Type of planner
group_l.set_planner_id(planner)
group_l.set_pose_reference_frame("yumi_body")

# Setting the workspace
group_l.set_workspace(ws=ws_L)

# Replanning
group_l.allow_replanning(True)
group_l.set_goal_tolerance(0.005)
group_l.set_num_planning_attempts(planning_attempts)
group_l.set_planning_time(planning_time)

# Right arm
group_r = MoveGroupCommander("right_arm")
# Type of planner
# group_r.set_planner_id(planner)
group_r.set_pose_reference_frame("yumi_body ...

Hi i'm a newbie of ROS and C++, i worked with python for some months and i would like to start to use C++ which is well supported. I am using ROS Kinetic with Ubuntu 16.04 and Qt-creator with ROS plugin as IDE. I created a new workspace with: catkin build and a new package with: catkin_create_pkg test1 roscpp rospy std_msgs. I opened the CMakeLists.txt inside the package and i modified this in this way:

cmake_minimum_required(VERSION 3.0.2)
project(test1)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
)
find_package(moveit_ros_planning_interface)
include_directories(${moveit_ros_planning_interface_DIR})
find_package(Eigen3 REQUIRED)
include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS})

## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES test1
#  CATKIN_DEPENDS roscpp rospy std_msgs moveit_ros_planning_interface
#  DEPENDS system_lib
)
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)
add_executable(test1 src/moveit_tutorial.cpp)

I created this simple C++ script like the moveit tutorials in order to practice:

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>


int main(int argc, char **argv)
{
    // Inizializzazione nodo ROS
    ros::init(argc, argv, "moveit_tutorial");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    ROS_INFO("HELLO! NODO INIT");

    // Nome del gruppo
    static const std::string PLANNING_GROUP = "manipulator";

    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

    // Raw pointers are frequently used to refer to the planning group for improved performance.
    const robot_state::JointModelGroup* joint_model_group =
            move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);


    geometry_msgs::Pose target_pose1;
    target_pose1.orientation.w = 1.0;
    target_pose1.position.x = 0.10;
    target_pose1.position.y = -0.1;
    target_pose1.position.z = 0.3;

    move_group.setPoseTarget(target_pose1);

    moveit::planning_interface::MoveGroupInterface::Plan plan1;

    bool success = (move_group.plan(plan1) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    ROS_INFO_NAMED("tutorial", "visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");
    // usa move_group.move() quando usi un vero robot


    // Pianificare un joint-space goal
    // RobotState oggetto che contiene tutte le posizioni attuali pos/vel/acc
    moveit::core::RobotStatePtr current_state = move_group.getCurrentState();

    // Current set of joint values
    std::vector<double> joint_group_position;
    // copia da current state i valori dei giunti
    current_state->copyJointGroupPositions(joint_model_group, joint_group_position);

    // modifico un valore di un giunto
    joint_group_position[0] = -1.0;
    move_group.setJointValueTarget(joint_group_position);
    moveit::planning_interface::MoveGroupInterface::Plan plan2;
    success = (move_group.plan(plan2) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    ROS_INFO("HA AVUTO SUCCESSO? %s", success ? "SI" : "NO");
    if (success)
    {
        move_group.execute(plan2);
    }
    ros::shutdown();
    return 0;
}

On the IDE i receive this error: moveit_tutorial.cpp:40:43: error: member reference type 'robot_state::RobotStatePtr' (aka 'int') is not a pointer

And after sourcing the workspace and building i received:

Errors     << test1:make /home/kappa95/ur5_tests/logs/test1/build.make.004.log 
In file included from /opt/ros/kinetic/include/moveit/macros/class_forward.h:40:0,
                 from /opt/ros/kinetic/include/moveit/move_group_interface/move_group_interface.h:41,
                 from /home/kappa95/ur5_tests/src/test1/src/moveit_tutorial.cpp:13:
/opt/ros/kinetic ...