Skip to content
Snippets Groups Projects
Commit 38734902 authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

velocity constraint planning, retreival and logging of robot state

parent 92977c1f
No related branches found
No related tags found
No related merge requests found
...@@ -12,6 +12,7 @@ find_package(catkin REQUIRED ...@@ -12,6 +12,7 @@ find_package(catkin REQUIRED
COMPONENTS controller_manager COMPONENTS controller_manager
effort_controllers effort_controllers
gazebo_ros gazebo_ros
genmsg
joint_state_controller joint_state_controller
joint_state_publisher joint_state_publisher
joint_trajectory_controller joint_trajectory_controller
...@@ -46,6 +47,7 @@ catkin_package(CATKIN_DEPENDS ...@@ -46,6 +47,7 @@ catkin_package(CATKIN_DEPENDS
hardware_interface hardware_interface
pluginlib pluginlib
#DEPENDS #DEPENDS
#libpaho-mqtt
# system_lib # system_lib
) )
...@@ -58,20 +60,22 @@ include_directories(${catkin_INCLUDE_DIRS}) ...@@ -58,20 +60,22 @@ include_directories(${catkin_INCLUDE_DIRS})
# add custom controller as library # add custom controller as library
add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp) add_library(${PROJECT_NAME}_controllers_lib src/joint_position_controller.cpp)
#add_library(SafetyEnvironmentCreator src/SafetyEnvironmentCreator.cpp)
# Specify libraries to link a library or executable target against # Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES}) target_link_libraries(${PROJECT_NAME}_controllers_lib ${catkin_LIBRARIES})
#target_link_libraries(SafetyEnvironmentCreator ${catkin_LIBRARIES} ${Boost_LIBRARIES})
add_executable(robot_control_node src/robot_control_node.cpp) add_executable(robot_control_node src/robot_control_node.cpp)
add_executable(box_publisher_node src/box_publisher.cpp) add_executable(box_publisher_node src/box_publisher.cpp)
add_executable(robot_state_initializer_node src/robot_state_initializer.cpp) add_executable(robot_state_initializer_node src/robot_state_initializer.cpp)
add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp src/SafetyEnvironmentCreator.cpp) add_executable(SafetyAwarePlanner src/SafetyAwarePlanner.cpp src/SafetyEnvironmentCreator.cpp)
add_executable(SampleConstraintPlanner src/SampleConstraintPlanner.cpp)
add_executable(RobotStateProvider src/RobotStateProvider.cpp)
add_executable(TimedCartesianPlanner src/TimedCartesianPlanner.cpp)
target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES}) target_link_libraries(robot_control_node ${catkin_LIBRARIES} boost_filesystem ${JSONCPP_LIBRARIES})
target_link_libraries(box_publisher_node ${catkin_LIBRARIES}) target_link_libraries(box_publisher_node ${catkin_LIBRARIES})
target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES}) target_link_libraries(robot_state_initializer_node ${catkin_LIBRARIES})
target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES}) target_link_libraries(SafetyAwarePlanner ${catkin_LIBRARIES})
#target_link_libraries(constraint_planner ${catkin_LIBRARIES} ${Boost_LIBRARIES} SafetyEnvironmentCreator) target_link_libraries(SampleConstraintPlanner ${catkin_LIBRARIES} ${Boost_LIBRARIES})
target_link_libraries(RobotStateProvider ${catkin_LIBRARIES})
target_link_libraries(TimedCartesianPlanner ${catkin_LIBRARIES})
\ No newline at end of file
<launch>
<param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
<!-- GAZEBO arguments -->
<arg name="paused" default="false" />
<arg name="use_sim_time" default="true" />
<arg name="gui" default="true" />
<arg name="headless" default="false" />
<arg name="debug" default="false" />
<arg name="load_gripper" default="true" />
<!--launch GAZEBO with own world configuration -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)" />
<arg name="use_sim_time" value="$(arg use_sim_time)" />
<arg name="headless" value="$(arg headless)" />
</include>
<node name="rqt_console" pkg="rqt_console" type="rqt_console" />
<!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
<!-- load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
<node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />
<!-- convert joint states to TF transforms for rviz, etc -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<include file="$(find panda_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true" />
<arg name="load_gripper" value="$(arg load_gripper)" />
</include>
<include file="$(find panda_moveit_config)/launch/move_group.launch">
<arg name="load_gripper" value="$(arg load_gripper)" />
</include>
<group if="$(arg gui)">
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
</group>
<node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
<!-- launch robot control node for moveit motion planning -->
<node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />
<!-- load (not start!) custom joint position controller -->
<node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />
<!-- run custom node for automatic intialization -->
<node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
<node name="TimedCartesianPlanner" pkg="panda_simulation" type="TimedCartesianPlanner" respawn="false" output="screen"/>
<node name="RobotStateProvider" pkg="panda_simulation" type="RobotStateProvider"/>
</launch>
//
// Created by sebastian on 31.03.20.
//
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <gazebo_msgs/LinkStates.h>
#include <MQTTClient.h>
// CONFIGURATION START
// ^^^^^^^^^^^^^^^^^^^
// configure the number of not redirected messages between redirected messages
const int message_redirect_rate = 100;
// values to export
bool export_position = true;
bool export_orientation = true;
bool export_twist_linear = true;
bool export_twist_angular = true;
bool export_names = true;
// CONFIGURATION END
// ^^^^^^^^^^^^^^^^^
int current_redirect = 0;
bool isInitialized = false;
/*
* logs to its own file in /.ros/logs (configured in launch-file)
*/
void exportMessageToLog(const gazebo_msgs::LinkStates & msg)
{
if(export_names){
ROS_INFO_STREAM("<<< LINK NAMES >>>");
for(int i = 0; i < msg.name.size(); i++)
{
ROS_INFO_STREAM("[" << i << "] " << msg.name.at(i));
}
}
if(export_position){
ROS_INFO_STREAM("<<< POSITIONS BY LINK >>>");
for(int i = 0; i < msg.pose.size(); i++)
{
ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).position.x);
ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).position.y);
ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).position.z);
}
}
if(export_orientation){
ROS_INFO_STREAM("<<< ORIENTATIONS BY LINK >>>");
for(int i = 0; i < msg.pose.size(); i++)
{
ROS_INFO_STREAM("[" << i << "] [w] " << msg.pose.at(i).orientation.w);
ROS_INFO_STREAM("[" << i << "] [x] " << msg.pose.at(i).orientation.x);
ROS_INFO_STREAM("[" << i << "] [y] " << msg.pose.at(i).orientation.y);
ROS_INFO_STREAM("[" << i << "] [z] " << msg.pose.at(i).orientation.z);
}
}
if(export_twist_linear){
ROS_INFO_STREAM("<<< LINEAR TWISTS BY LINK >>>");
for(int i = 0; i < msg.twist.size(); i++)
{
ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).linear.x);
ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).linear.y);
ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).linear.z);
}
}
if(export_twist_angular){
ROS_INFO_STREAM("<<< ANGULAR TWISTS BY LINK >>>");
for(int i = 0; i < msg.twist.size(); i++)
{
ROS_INFO_STREAM("[" << i << "] [x] " << msg.twist.at(i).angular.x);
ROS_INFO_STREAM("[" << i << "] [y] " << msg.twist.at(i).angular.y);
ROS_INFO_STREAM("[" << i << "] [z] " << msg.twist.at(i).angular.z);
}
}
}
void providerCallback(const gazebo_msgs::LinkStates & msg)
{
if(current_redirect != 0 && current_redirect != message_redirect_rate){
//std::cout << "Ignoring message (redirect: " << current_redirect << " )" << std::endl;
current_redirect++;
return;
}
if(current_redirect == 0 || current_redirect == message_redirect_rate){
//std::cout << "Redirecting message:" << msg.pose.at(1).position << std::endl;
exportMessageToLog(msg);
if(current_redirect == message_redirect_rate){
current_redirect = 1;
} else{
current_redirect++;
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "listener");
ros::NodeHandle n;
n.getParam("ready", isInitialized);
// make sure the robot is initialized
while(!isInitialized){
//std::cout << "Waiting" << std::endl;
n.getParam("tud_planner_ready", isInitialized);
}
ros::Subscriber sub = n.subscribe("/gazebo/link_states", 10000, providerCallback);
ros::spin();
return 0;
}
\ No newline at end of file
//
// Created by sebastian on 31.03.20.
//
#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>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
/**
* allows time/velocity-constraint planning for cartesian paths
*/
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
// wait for robot init of robot_state_initializer
std::cout << ">>>>>>>>>>>>>>>>> SLEEPING <<<<<<<<<<<<<<<< " << std::endl;
ros::Duration(5.0).sleep();
std::cout << ">>>>>>>>>>>>>>>>> WAKING UP <<<<<<<<<<<<<<<< " << std::endl;
node_handle.setParam("tud_planner_ready", true);
// Visualization Setup
namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();
visual_tools.loadRemoteControl();
Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
// Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
visual_tools.trigger();
moveit::planning_interface::MoveGroupInterface group("panda_arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
moveit::planning_interface::MoveGroupInterface::Plan plan;
group.setStartStateToCurrentState();
std::vector<geometry_msgs::Pose> waypoints;
geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;
target_pose_1.position.z = start_pose.position.z - 0.2;
target_pose_1.position.y = start_pose.position.y + 0.2;
target_pose_1.position.x = start_pose.position.x + 0.2;
waypoints.push_back(target_pose_1);
geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose;
target_pose_2.position.z = start_pose.position.z + 0.2;
target_pose_1.position.y = start_pose.position.y - 0.2;
target_pose_2.position.x = start_pose.position.x - 0.2;
waypoints.push_back(target_pose_2);
moveit_msgs::RobotTrajectory trajectory_msg;
group.setPlanningTime(10.0);
double fraction = group.computeCartesianPath(waypoints,0.01,0.0,trajectory_msg, false);
// The trajectory needs to be modified so it will include velocities as well.
// First to create a RobotTrajectory object
robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
// Second get a RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
// Third create a IterativeParabolicTimeParameterization object
trajectory_processing::IterativeParabolicTimeParameterization iptp;
// Fourth compute computeTimeStamps
bool success = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
rt.getRobotTrajectoryMsg(trajectory_msg);
//std::cout << ">>>>>>>>>>>>>>>>> data before: " << trajectory_msg << std::endl;
// adjust velocities, accelerations and time_from_start
for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++)
{
trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(2);
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++)
{
if(trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0){
trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) /= 2;
//trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) = 0;
}
}
for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++)
{
trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
}
}
//std::cout << ">>>>>>>>>>>>>>>>> data after: " << trajectory_msg << std::endl;
plan.trajectory_ = trajectory_msg;
ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);
group.execute(plan);
ros::shutdown();
return 0;
}
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment