microsoft
/
AI-Robot-Challenge-Lab
зеркало из https://github.com/microsoft/AI-Robot-Challenge-Lab.git
1
0
Форкнуть 0
Код Задачи Пакеты Проекты Релизы Вики Активность

merge original code

This commit is contained in:
Pablo Iñigo Blasco 2019-10-21 20:12:01 +02:00
Родитель f34abb1f31 2f5a70178f
Коммит c9acf9e317
99 изменённых файлов: 122891 добавлений и 5 удалений
Показать статистику Скачать Patch файл Скачать Diff файл Показать все файлы Свернуть все файлы

0
.catkin_tools/CATKIN_IGNORE Normal file
Просмотреть файл

13
.catkin_tools/README Normal file
Просмотреть файл

@ -0,0 +1,13 @@
# Catkin Tools Metadata
This directory was generated by catkin_tools and it contains persistent
configuration information used by the `catkin` command and its sub-commands.
Each subdirectory of the `profiles` directory contains a set of persistent
configuration options for separate profiles. The default profile is called
`default`. If another profile is desired, it can be described in the
`profiles.yaml` file in this directory.
Please see the catkin_tools documentation before editing any files in this
directory. Most actions can be performed with the `catkin` command-line
program.

1
.catkin_tools/VERSION Normal file
Просмотреть файл

@ -0,0 +1 @@
0.4.4

6
.gitignore поставляемый
Просмотреть файл

@ -86,11 +86,6 @@ StyleCopReport.xml
*.svclog
*.scc
.catkin_tools
build
devel
logs
# Chutzpah Test files
_Chutzpah*
@ -100,6 +95,7 @@ ipch/
*.ncb
*.opendb
*.opensdf
*.sdf
*.cachefile
*.VC.db
*.VC.VC.opendb

74
lab_documents/README_Draft.md Normal file
Просмотреть файл

@ -0,0 +1,74 @@
# AI Robot Challenge
Introduction and what will be achieved.. TODO
# Getting started with robotics
## Introduction to technologies used
### **ROS**
ROS (Robot Operating System) is robotics middleware licensed under an open source, BSD license. Althought ROS is not an Operative System, it provides libraries, hardware abstraction, device drivers, visualizers, message-passing, package management, and other tools to help software developers create robot applications.
Last Release: ROS Melodic Morenia. Supported on Ubuntu Artful and Bionic, along with Debian Stretch.
Supports collaboration throught Repositories.
ROS is a distributed framework of processes (aka Nodes) that enables executables to be individually added, which makes the framework very modular. These processes can be grouped into Packages and Stacks, which can be easily shared and distributed.
**Languages:** Python, C++, and Lisp
**System Requirements:** supports Unix-based systems, primarly Ubuntu and Mac OS X systems, but the community has been adding support to Fedora, Gentoo, Arch Linux and other Linux platforms.
Althought there are some Robotics Kits available in the market, ROS is quickly becoming the new standard for both industrial and research robotics as it integrates both hardware and software in their solution for industrial applications.
### **Gazebo**
Gazebo is a 3D robot simulation tool that seamlessly integrates with ROS, which allows to run the Gazebo server in a ROS environment. Gazebo allows to build 3D scenarios on your computer with robots, using obstacles and other objects. This allows to test robots in complex or dangerous scenarios without any harm to the real robot. Most of the time it is faster and cost effective to simply run a simulator instead of starting the whole scenario on a real robot. The testing models used by the simulators can be created using XML or a graphical model editor. It uses a physics engines for realistic movements called ODE (Open Dynamics Engine).
Gazebo has two main components: the server which acts like a back-end to compute all the logic for your testing scenarios and a client which works as a graphical front-end. This is very useful as sometimes you might only want to execute the tests without the UI in order to speed up the execution process.
Gazebo was used to simulate the atlas robot for the Virtual Robotics Challenge (the precursor to the DARPA robotics challenge), which required to build a software simulation of humanoid rescue work.
There are other commercial versions for robotics simulations but Gazebo is a strong competitor that is free to use under Apache 2.0 license.
**Languages:** C++ API
**System Requirements:** Linux
**Robotics Middleware support:** ROS, Player, Sockets (protobuf messages)
### **RViz**
RViz is an Open Source 3D visualizer for the Robot Operating System (ROS) framework.
Uses sensors data and custom visualization markers to develop robot capabilities in a 3d environment.
Features:
- Motion planning
- Object detection
- Calibration
- Debugging
- RViz visualization widget
- 3D stereo rendering
RViz provides a CLI tool that lets you execute python or c++ scripts with controls.
### **Sawyer**
Sawyer is an integrated collaborative robot (aka cobot) solution designed with embedded vision, ClickSmart grippers, and high resolution force control. The robot purpose is to automate specific industrial repetive tasks, it comes with an arm that has a gripper which can be easily replaced by one of the available options from the ClickSmart Gripper Kit.
Features:
- Sawyer comes with highly sensitive torque sensors embedded into every joint, this allows you to control force where delicate part insertion is critical, or use force if needed. It can maneuver into tight spaces and it has a long reach of 1260 mm (max).
- Comes with an embedded vision system used for the robot positioning, it also allows external vision systems like cameras.
- The software that comes with the robot is continuosly updated.
- Fast to deploy as many pieces are plug&play, ready to use and with integrated sensors.
### **MoveIt!**
MoveIt is software for motion and path planning. Users can access actions and services using: C++, Python, Through a GUI.
Features:
- Motion planning
- Manipulation
- 3D perception
- Kinematics
- Control and navigation
Underneath it uses OMPL (Open Motion Planning Library) and requires a controller to send messages to the hardware. MoveIt provides a Fake Controller to interact with the hardware using ROS messages but you can replace the fake robot controller in MoveIt with your own plugin which controls the robot if needed.
The planning scene feature allows to monitor the state, sensor and world geometry information.
## Getting started
# Bringing your robot to life
# Making your robot intelligent with Microsoft AI

201
sawyer_ik_5d/CMakeLists.txt Normal file
Просмотреть файл

@ -0,0 +1,201 @@
cmake_minimum_required(VERSION 2.8.3)
project(sawyer_ik_5d)
## 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
kdl_parser
moveit_msgs
roscpp
)
find_package(orocos_kdl)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES sawyer_ik_5d
# CATKIN_DEPENDS orocos_kdl roscpp
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/sawyer_ik_5d.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_node src/sawyer_ik_5d_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_sawyer_ik_5d.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

0
sawyer_ik_5d/include/sawyer_ik_5d/sawyer_ik_5d.h Normal file
Просмотреть файл

71
sawyer_ik_5d/package.xml Normal file
Просмотреть файл

@ -0,0 +1,71 @@
<?xml version="1.0"?>
<package format="2">
<name>sawyer_ik_5d</name>
<version>0.0.0</version>
<description>The sawyer_ik_5d package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="pblasco@plainconcepts.com">Pablo Inigo Blasco</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>copyright</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/sawyer_ik_5d</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>orocos_kdl</build_depend>
<build_depend>kdl_parser</build_depend>
<build_depend>moveit_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_export_depend>orocos_kdl</build_export_depend>
<build_export_depend>kdl_parser</build_export_depend>
<build_export_depend>moveit_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<exec_depend>orocos_kdl</exec_depend>
<exec_depend>kdl_parser</exec_depend>
<exec_depend>moveit_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

619
sawyer_ik_5d/src/sawyer_ik_5d_node.cpp Normal file
Просмотреть файл

@ -0,0 +1,619 @@
#include <urdf/model.h>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <ros/ros.h>
#include <moveit_msgs/GetPositionIK.h>
using namespace KDL;
using namespace Eigen;
using namespace ros;
using namespace std;
#define SAWYER_JOINT_COUNT 6
ChainIkSolverPos_LMA *ikSolverPos;
ChainFkSolverPos_recursive *fkSolverPos;
Tree tree;
Chain chain;
Eigen::Matrix<double,6/*pose params*/,1> L;
vector<string> joint_names;
JntArray q_current(SAWYER_JOINT_COUNT);
ros::ServiceServer ikservice;
bool initialized= false;
void getParameters()
{
NodeHandle nh;
////////////////////////////////////////////////////////////
// Get robot description
////////////////////////////////////////////////////////////
string description;
if(!nh.getParam("/robot_description", description))
{
ROS_ERROR("Parameter not set: /robot_description");
exit(-1);
}
////////////////////////////////////////////////////////////
// Create robot model
////////////////////////////////////////////////////////////
urdf::Model model;
if(!model.initString(description))
{
ROS_ERROR("Could not initialize robot model");
exit(-1);
}
////////////////////////////////////////////////////////////
// Get tip link name and verify it exists in the model
////////////////////////////////////////////////////////////
string tip_link_name;
if(!nh.getParam("tip_link_name", tip_link_name))
{
tip_link_name = "right_hand_camera_optical";
ROS_WARN("Parameter not set: %s/tip_link_name, default value -> %s", nh.getNamespace().c_str(), tip_link_name.c_str());
//exit(-1);
}
if(model.links_.find(tip_link_name) == model.links_.end())
{
ROS_ERROR("The link specified in %s/tip_link_name does not exist", nh.getNamespace().c_str());
exit(-1);
}
////////////////////////////////////////////////////////////
// Get revolute joints
////////////////////////////////////////////////////////////
vector<boost::shared_ptr<urdf::Joint> > model_joints;
for(boost::shared_ptr<const urdf::Link> current_link = model.getLink(tip_link_name); current_link->parent_joint != NULL; current_link = current_link->getParent())
{
if(current_link->parent_joint->type == urdf::Joint::REVOLUTE)
{
model_joints.insert(model_joints.begin(), current_link->parent_joint);
}
}
if(model_joints.size() != SAWYER_JOINT_COUNT)
{
ROS_ERROR("The robot model must have %d revolute joints, found %d", SAWYER_JOINT_COUNT, (int)model_joints.size());
exit(-1);
}
////////////////////////////////////////////////////////////
// Configure servo limits
////////////////////////////////////////////////////////////
// for(int joint_i = 0; joint_i < SAWYER_JOINT_COUNT; joint_i++)
// {
// float lowerLimit = model_joints[joint_i]->limits->lower;
// float upperLimit = model_joints[joint_i]->limits->upper;
// }
////////////////////////////////////////////////////////////
// Store joint names
////////////////////////////////////////////////////////////
for(int joint_i = 0; joint_i < SAWYER_JOINT_COUNT; joint_i++)
{
joint_names.push_back(model_joints[joint_i]->name);
}
////////////////////////////////////////////////////////////
// Create KDL tree
////////////////////////////////////////////////////////////
if(!kdl_parser::treeFromString(description, tree))
{
ROS_ERROR("Failed to construct kdl tree");
exit(-1);
}
if(!tree.getChain(model.getRoot()->name, tip_link_name, chain))
{
ROS_ERROR("Failed to make chain from tree");
exit(-1);
}
}
void initialize_ik()
{
getParameters();
ROS_INFO("creating solvers");
// Create KDL solvers
L(0) = 1; L(1) = 1; L(2) = 1; L(3) = 1; L(4) = 1; L(5) = 0 ;
ikSolverPos = new KDL::ChainIkSolverPos_LMA(chain, L);
fkSolverPos = new KDL::ChainFkSolverPos_recursive(chain);
}
bool ik(moveit_msgs::GetPositionIK::Request& request, moveit_msgs::GetPositionIK::Response& response)
{
ROS_INFO("ik request");
if(!initialized)
{
ROS_INFO("initializing");
initialize_ik();
ROS_INFO("initialized");
}
ROS_INFO("reading inputs");
for(int i=0;i<SAWYER_JOINT_COUNT;i++)
{
q_current(i) = request.ik_request.robot_state.joint_state.position[i];
}
const geometry_msgs::Point& p = request.ik_request.pose_stamped.pose.position;
const geometry_msgs::Quaternion& q = request.ik_request.pose_stamped.pose.orientation;
Frame target_frame (Rotation::Quaternion(q.x,q.y,q.z,q.w),Vector(p.x,p.y,p.z));
ROS_INFO("computing ik, target %lf %lf %lf", target_frame.p[0],target_frame.p[1],target_frame.p[2]);
JntArray q_res;
int result = ikSolverPos->CartToJnt(q_current, target_frame,q_res);
ROS_INFO("IK RESULT: %d", result);
//0 if successful, -1 the gradient of $ E $ towards the joints is to small, -2 if joint position increments are to small, -3 if number of iterations is exceeded.
sensor_msgs::JointState& respjoints= response.solution.joint_state;
respjoints.name= request.ik_request.robot_state.joint_state.name;
for(int i=0;i<SAWYER_JOINT_COUNT;i++)
respjoints.position.push_back(q_res(i));
return true;
}
int main(int argc, char** argv)
{
// Init ROS node
ros::init(argc, argv, "sawyer_ik_5d");
// Get node handle
NodeHandle nh("~");
ikservice = nh.advertiseService("ik", ik);
ros::spin();
}
/*
moveit_msgs/PositionIKRequest ik_request
string group_name
moveit_msgs/RobotState robot_state
sensor_msgs/JointState joint_state
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string[] name
float64[] position
float64[] velocity
float64[] effort
sensor_msgs/MultiDOFJointState multi_dof_joint_state
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string[] joint_names
geometry_msgs/Transform[] transforms
geometry_msgs/Vector3 translation
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation
float64 x
float64 y
float64 z
float64 w
geometry_msgs/Twist[] twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
geometry_msgs/Wrench[] wrench
geometry_msgs/Vector3 force
float64 x
float64 y
float64 z
geometry_msgs/Vector3 torque
float64 x
float64 y
float64 z
moveit_msgs/AttachedCollisionObject[] attached_collision_objects
string link_name
moveit_msgs/CollisionObject object
byte ADD=0
byte REMOVE=1
byte APPEND=2
byte MOVE=3
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string id
object_recognition_msgs/ObjectType type
string key
string db
shape_msgs/SolidPrimitive[] primitives
uint8 BOX=1
uint8 SPHERE=2
uint8 CYLINDER=3
uint8 CONE=4
uint8 BOX_X=0
uint8 BOX_Y=1
uint8 BOX_Z=2
uint8 SPHERE_RADIUS=0
uint8 CYLINDER_HEIGHT=0
uint8 CYLINDER_RADIUS=1
uint8 CONE_HEIGHT=0
uint8 CONE_RADIUS=1
uint8 type
float64[] dimensions
geometry_msgs/Pose[] primitive_poses
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
shape_msgs/Mesh[] meshes
shape_msgs/MeshTriangle[] triangles
uint32[3] vertex_indices
geometry_msgs/Point[] vertices
float64 x
float64 y
float64 z
geometry_msgs/Pose[] mesh_poses
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
shape_msgs/Plane[] planes
float64[4] coef
geometry_msgs/Pose[] plane_poses
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
byte operation
string[] touch_links
trajectory_msgs/JointTrajectory detach_posture
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string[] joint_names
trajectory_msgs/JointTrajectoryPoint[] points
float64[] positions
float64[] velocities
float64[] accelerations
float64[] effort
duration time_from_start
float64 weight
bool is_diff
moveit_msgs/Constraints constraints
string name
moveit_msgs/JointConstraint[] joint_constraints
string joint_name
float64 position
float64 tolerance_above
float64 tolerance_below
float64 weight
moveit_msgs/PositionConstraint[] position_constraints
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string link_name
geometry_msgs/Vector3 target_point_offset
float64 x
float64 y
float64 z
moveit_msgs/BoundingVolume constraint_region
shape_msgs/SolidPrimitive[] primitives
uint8 BOX=1
uint8 SPHERE=2
uint8 CYLINDER=3
uint8 CONE=4
uint8 BOX_X=0
uint8 BOX_Y=1
uint8 BOX_Z=2
uint8 SPHERE_RADIUS=0
uint8 CYLINDER_HEIGHT=0
uint8 CYLINDER_RADIUS=1
uint8 CONE_HEIGHT=0
uint8 CONE_RADIUS=1
uint8 type
float64[] dimensions
geometry_msgs/Pose[] primitive_poses
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
shape_msgs/Mesh[] meshes
shape_msgs/MeshTriangle[] triangles
uint32[3] vertex_indices
geometry_msgs/Point[] vertices
float64 x
float64 y
float64 z
geometry_msgs/Pose[] mesh_poses
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64 weight
moveit_msgs/OrientationConstraint[] orientation_constraints
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
string link_name
float64 absolute_x_axis_tolerance
float64 absolute_y_axis_tolerance
float64 absolute_z_axis_tolerance
float64 weight
moveit_msgs/VisibilityConstraint[] visibility_constraints
uint8 SENSOR_Z=0
uint8 SENSOR_Y=1
uint8 SENSOR_X=2
float64 target_radius
geometry_msgs/PoseStamped target_pose
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
int32 cone_sides
geometry_msgs/PoseStamped sensor_pose
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64 max_view_angle
float64 max_range_angle
uint8 sensor_view_direction
float64 weight
bool avoid_collisions
string ik_link_name
geometry_msgs/PoseStamped pose_stamped
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
string[] ik_link_names
geometry_msgs/PoseStamped[] pose_stamped_vector
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
duration timeout
int32 attempts
---
moveit_msgs/RobotState solution
sensor_msgs/JointState joint_state
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string[] name
float64[] position
float64[] velocity
float64[] effort
sensor_msgs/MultiDOFJointState multi_dof_joint_state
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string[] joint_names
geometry_msgs/Transform[] transforms
geometry_msgs/Vector3 translation
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation
float64 x
float64 y
float64 z
float64 w
geometry_msgs/Twist[] twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
geometry_msgs/Wrench[] wrench
geometry_msgs/Vector3 force
float64 x
float64 y
float64 z
geometry_msgs/Vector3 torque
float64 x
float64 y
float64 z
moveit_msgs/AttachedCollisionObject[] attached_collision_objects
string link_name
moveit_msgs/CollisionObject object
byte ADD=0
byte REMOVE=1
byte APPEND=2
byte MOVE=3
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string id
object_recognition_msgs/ObjectType type
string key
string db
shape_msgs/SolidPrimitive[] primitives
uint8 BOX=1
uint8 SPHERE=2
uint8 CYLINDER=3
uint8 CONE=4
uint8 BOX_X=0
uint8 BOX_Y=1
uint8 BOX_Z=2
uint8 SPHERE_RADIUS=0
uint8 CYLINDER_HEIGHT=0
uint8 CYLINDER_RADIUS=1
uint8 CONE_HEIGHT=0
uint8 CONE_RADIUS=1
uint8 type
float64[] dimensions
geometry_msgs/Pose[] primitive_poses
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
shape_msgs/Mesh[] meshes
shape_msgs/MeshTriangle[] triangles
uint32[3] vertex_indices
geometry_msgs/Point[] vertices
float64 x
float64 y
float64 z
geometry_msgs/Pose[] mesh_poses
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
shape_msgs/Plane[] planes
float64[4] coef
geometry_msgs/Pose[] plane_poses
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
byte operation
string[] touch_links
trajectory_msgs/JointTrajectory detach_posture
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string[] joint_names
trajectory_msgs/JointTrajectoryPoint[] points
float64[] positions
float64[] velocities
float64[] accelerations
float64[] effort
duration time_from_start
float64 weight
bool is_diff
moveit_msgs/MoveItErrorCodes error_code
int32 SUCCESS=1
int32 FAILURE=99999
int32 PLANNING_FAILED=-1
int32 INVALID_MOTION_PLAN=-2
int32 MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE=-3
int32 CONTROL_FAILED=-4
int32 UNABLE_TO_AQUIRE_SENSOR_DATA=-5
int32 TIMED_OUT=-6
int32 PREEMPTED=-7
int32 START_STATE_IN_COLLISION=-10
int32 START_STATE_VIOLATES_PATH_CONSTRAINTS=-11
int32 GOAL_IN_COLLISION=-12
int32 GOAL_VIOLATES_PATH_CONSTRAINTS=-13
int32 GOAL_CONSTRAINTS_VIOLATED=-14
int32 INVALID_GROUP_NAME=-15
int32 INVALID_GOAL_CONSTRAINTS=-16
int32 INVALID_ROBOT_STATE=-17
int32 INVALID_LINK_NAME=-18
int32 INVALID_OBJECT_NAME=-19
int32 FRAME_TRANSFORM_FAILURE=-21
int32 COLLISION_CHECKING_UNAVAILABLE=-22
int32 ROBOT_STATE_STALE=-23
int32 SENSOR_INFO_STALE=-24
int32 NO_IK_SOLUTION=-31
int32 val
*/

0
setup/CATKIN_IGNORE Normal file
Просмотреть файл

285
setup/docker/dockerfile Normal file
Просмотреть файл

@ -0,0 +1,285 @@
#
# ROS image for Sawyer development
#
# ***
# *** Sample Docker commands
# ***
# Run the following command from the same directory as the Dockerfile
# > docker build --squash -t robotchallenge:latest .
# verify docker version
# > docker version
# To run the new image and create a container in interactive mode )-it)
# > docker run -it robotchallenge
# LIST all Containers
# > docker ps -a
# DELETE the CONTAINER ( if status is exited)
# > docker rm $(docker ps -a -f status=exited -q)
# LIST all Images
# > docker image ls
# DELETE Images all images
# > docker rmi $(docker images -a -q)
# ***
# *** End Sample Docker Commands
# ***
# Pull base image.
FROM ubuntu:16.04
LABEL author "Paul Stubbs (pstubbs@microsoft.com)"
LABEL description "Microsoft Robot Challenge 2018"
LABEL version "0.0"
# ARGS
#https://en.wikipedia.org/wiki/Marvin_the_Paranoid_Android
ARG username="marvin"
ARG password="BigHero6"
ARG groupname="robotgroup"
ARG groupid="1024"
ARG userid="1024"
# Create the user account
RUN echo '***\n***\n***\n***\nCreate the user account\n***\n***\n***\n***'
RUN groupadd -g ${groupid} ${groupname} && \
useradd --shell /bin/bash --password ${password} --gid ${groupname} --groups sudo --create-home ${username}
USER ${username}
WORKDIR /home/${username}
RUN HOME=/home/${username}
# Run in noninteractive mode to supress some prompts
ENV DEBIAN_FRONTEND noninteractive
# Set the default shell to bash
SHELL ["/bin/bash", "-c"]
###
USER root
###
# Update to lateset software lists
RUN echo '***\n***\n***\n***\nUpdate to lateset software lists\n***\n***\n***\n***'
RUN apt-get update -y
# Install some common CLI tools
RUN apt-get install -y apt-utils wget software-properties-common sudo
#
#
#
# Install ROS
RUN echo '***\n***\n***\n***\nInstall ROS\n***\n***\n***\n***'
#
#
# http://sdk.rethinkrobotics.com/intera/Workstation_Setup
# Configure Ubuntu repositories. Setup sources.list
RUN ["/bin/sh", "-c", "echo \"deb http://packages.ros.org/ros/ubuntu xenial main\" > /etc/apt/sources.list.d/ros-latest.list"]
# Setup keys
RUN apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
# Install ROS Kinetic Desktop FUll
RUN echo '***\n***\n***\n***\n Install ROS Kinetic Desktop FUll \n***\n***\n***\n***'
RUN apt-get update && apt-get install -y ros-kinetic-desktop-full
# Initialize rosdep
RUN rosdep init
###
USER ${username}
###
RUN rosdep update
###
USER root
###
# Install rosinstall
RUN apt-get install python-rosinstall -y
###
USER ${username}
###
#
#
#
# Create Development Workspace
RUN echo '***\n***\n***\n***\nCreate Development Workspace\n***\n***\n***\n***'
#
#
#
# Create ROS Workspace
RUN mkdir -p ~/ros_ws/src && \
cd ~/ros_ws && \
source /opt/ros/kinetic/setup.bash && \
catkin_make
#
#
#
# Install Intera SDK Dependencies
#
#
#
# Install SDK Dependencies
###
USER root
###
# Update to lateset software lists
RUN apt-get update -y
RUN apt-get update && apt-get install -y \
git-core \
python-argparse \
python-wstool \
python-vcstools \
python-rosdep \
ros-kinetic-control-msgs \
ros-kinetic-joystick-drivers \
ros-kinetic-xacro \
ros-kinetic-tf2-ros \
ros-kinetic-rviz \
ros-kinetic-cv-bridge \
ros-kinetic-actionlib \
ros-kinetic-actionlib-msgs \
ros-kinetic-dynamic-reconfigure \
ros-kinetic-trajectory-msgs \
ros-kinetic-rospy-message-converter
###
USER ${username}
###
#
#
#
# Install Intera Robot SDK
#
#
#
RUN cd ~/ros_ws/src && \
wstool init . && \
git clone https://github.com/RethinkRobotics/sawyer_robot.git && \
wstool merge sawyer_robot/sawyer_robot.rosinstall && \
wstool update
# Source ROS Setup
RUN cd ~/ros_ws && \
source /opt/ros/kinetic/setup.bash && \
catkin_make
#
#
#
# Configure Robot Communication/ROS Workspace
#
#
#
# Copy the intera.sh script
# The intera.sh file already exists in intera_sdk repo,
# copy the file into your ros workspace.
RUN cp ~/ros_ws/src/intera_sdk/intera.sh ~/ros_ws
# Update the copy of the intera.sh file
# RUN cd ~/ros_ws
# Update ROS Distribution
RUN sed -i 's/ros_version="indigo"/ros_version="kinetic"/' ~/ros_ws/intera.sh
# Update the ROBOTS hostname
RUN sed -i 's/robot_hostname="robot_hostname.local"/robot_hostname="paule.local"/' ~/ros_ws/intera.sh
# TODO:// Need to figure out the docker networking to resolve hostname
# Update YOUR IP or Hostname. This must be resolvable to the Robot
# Choose one. Be sure to add or remove the leading # from the right ones
RUN sed -i 's/your_ip="192.168.XXX.XXX"/your_ip="192.168.XXX.XXX"/' ~/ros_ws/intera.sh
#RUN sed -i 's/#your_hostname="my_computer.local"/your_hostname="my_computer.local"/' intera.sh
# Setup and configure RVIZ
# TODO:// need to find a way to do this from the dockerfile
# http://sdk.rethinkrobotics.com/intera/Gazebo_Tutorial
#
#
#
# Configure Sawyer with Gazebo
#
#
#
# Install Prerequisites
###
USER root
###
# Update to lateset software lists
RUN apt-get update && apt-get install -y \
gazebo7 \
ros-kinetic-qt-build \
ros-kinetic-gazebo-ros-control \
ros-kinetic-gazebo-ros-pkgs \
ros-kinetic-ros-control \
ros-kinetic-control-toolbox \
ros-kinetic-realtime-tools \
ros-kinetic-ros-controllers \
ros-kinetic-xacro \
python-wstool \
ros-kinetic-tf-conversions \
ros-kinetic-kdl-parser \
ros-kinetic-sns-ik-lib
###
USER ${username}
###
# Install Sawyer Simulator files
RUN cd ~/ros_ws/src && \
git clone https://github.com/RethinkRobotics/sawyer_simulator.git && \
source /opt/ros/kinetic/setup.bash && \
wstool merge sawyer_simulator/sawyer_simulator.rosinstall && \
wstool update
# Build the Sources
RUN source /opt/ros/kinetic/setup.bash && \
cd ~/ros_ws && \
catkin_make
#
#
#
# Install other tools
#
#
#
###
USER root
###
#Add the path to ROS
RUN echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
# Clean up all the temp files
RUN apt-get clean && rm -rf /var/lib/apt/lists/* /tmp/* /var/tmp/*
# TODO:// install VSCode

1
setup/docker/readme.md Normal file
Просмотреть файл

@ -0,0 +1 @@
Setup instructions

1
setup/readme.md Normal file
Просмотреть файл

@ -0,0 +1 @@
Setup instructions

16
setup/ubuntu/.vscode/c_cpp_properties.json поставляемый Normal file
Просмотреть файл

@ -0,0 +1,16 @@
{
"configurations": [
{
"browse": {
"databaseFilename": "",
"limitSymbolsToIncludedHeaders": true
},
"includePath": [
"/home/marvin/ros_ws/devel/include",
"/opt/ros/kinetic/include",
"/usr/include"
],
"name": "Linux"
}
]
}

118
setup/ubuntu/.vscode/launch.json поставляемый Normal file
Просмотреть файл

@ -0,0 +1,118 @@
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"name": "Python: Current File",
"type": "python",
"request": "launch",
"program": "${file}",
"console":"integratedTerminal",
"preLaunchTask": "prerun"
},
{
"name": "Python: Attach",
"type": "python",
"request": "attach",
"localRoot": "${workspaceFolder}",
"remoteRoot": "${workspaceFolder}",
"port": 3000,
"secret": "my_secret",
"host": "localhost"
},
{
"name": "Python: Terminal (integrated)",
"type": "python",
"request": "launch",
"program": "${file}",
"console": "integratedTerminal"
},
{
"name": "Python: Terminal (external)",
"type": "python",
"request": "launch",
"program": "${file}",
"console": "externalTerminal"
},
{
"name": "Python: Django",
"type": "python",
"request": "launch",
"program": "${workspaceFolder}/manage.py",
"args": [
"runserver",
"--noreload",
"--nothreading"
],
"debugOptions": [
"RedirectOutput",
"Django"
]
},
{
"name": "Python: Flask (0.11.x or later)",
"type": "python",
"request": "launch",
"module": "flask",
"env": {
"FLASK_APP": "app.py"
},
"args": [
"run",
"--no-debugger",
"--no-reload"
]
},
{
"name": "Python: Module",
"type": "python",
"request": "launch",
"module": "module.name"
},
{
"name": "Python: Pyramid",
"type": "python",
"request": "launch",
"args": [
"${workspaceFolder}/development.ini"
],
"debugOptions": [
"RedirectOutput",
"Pyramid"
]
},
{
"name": "Python: Watson",
"type": "python",
"request": "launch",
"program": "${workspaceFolder}/console.py",
"args": [
"dev",
"runserver",
"--noreload=True"
]
},
{
"name": "Python: All debug Options",
"type": "python",
"request": "launch",
"pythonPath": "${config:python.pythonPath}",
"program": "${file}",
"module": "module.name",
"env": {
"VAR1": "1",
"VAR2": "2"
},
"envFile": "${workspaceFolder}/.env",
"args": [
"arg1",
"arg2"
],
"debugOptions": [
"RedirectOutput"
]
}
]
}

3
setup/ubuntu/.vscode/settings.json поставляемый Normal file
Просмотреть файл

@ -0,0 +1,3 @@
{
"python.pythonPath": "/usr/bin/python3.6"
}

13
setup/ubuntu/.vscode/tasks.json поставляемый Normal file
Просмотреть файл

@ -0,0 +1,13 @@
{
// See https://go.microsoft.com/fwlink/?LinkId=733558
// for the documentation about the tasks.json format
"version": "2.0.0",
"tasks": [
{
"label": "prerun",
"type": "shell",
"command": "cd ~/ros_ws && ./intera.sh",
"problemMatcher": []
}
]
}

15
setup/ubuntu/SawyerBot.bot Normal file
Просмотреть файл

@ -0,0 +1,15 @@
{
"name": "RoboticsBot",
"description": "",
"secretKey": "",
"services": [
{
"appId": "",
"id": "http://localhost:9000/api/messages",
"type": "endpoint",
"appPassword": "",
"endpoint": "http://localhost:9000/api/messages",
"name": "RoboticsBot"
}
]
}

31
setup/ubuntu/bot-stats-node.py Executable file
Просмотреть файл

@ -0,0 +1,31 @@
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from intera_core_msgs.msg import AssemblyState
import sys
import json
print 'bot-stats-node starting'
sub_once = None
def callback(data):
print 'demostats callback'
#rospy.loginfo("I heard %s",data.data)
print json.dumps(data.enabled)
sub_once.unregister()
def main():
print 'demostats listener'
rospy.init_node('Demo_Stats')
global sub_once
sub_once = rospy.Subscriber("/robot/state", AssemblyState, callback )
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__ == '__main__':
sys.exit(main())

45383
setup/ubuntu/json Normal file
Просмотреть файл

Разница между файлами не показана из-за своего большого размера Загрузить разницу

21
setup/ubuntu/readme.md Normal file
Просмотреть файл

@ -0,0 +1,21 @@
# Ubuntu Setup Instructions
## Run the Install Script
1. Clone the repo.
1. Navigate to `AI-Robot-Challenge/setup/ubuntu` in a Terminal window.
1. Run the following command `chmod +x robot-challenge-setup.sh`.
1. Run the shell script with the following command `./robot-challenge-setup.sh`
## Run the Bot Locally
1. Navigate to `AI-Robot-Challenge/setup/ubuntu` in a Terminal window.
1. Run the following command `python3.6 main.py`.
## Run the Emulator
1. Run `Bot Framework Emulator` from your applications.
1. Click **Open Bot** and select the bot configuration file: `AI-Robot-Challenge/setup/ubuntu/SawyerBot.bot`
1. You will be welcomed with the message `'Welcome to Sawyer Robot!'`.
1. Send the message `move arm` or `show stats` to trigger intents.

45383
setup/ubuntu/requests Normal file
Просмотреть файл

Разница между файлами не показана из-за своего большого размера Загрузить разницу

448
setup/ubuntu/robot-challenge-setup.sh Executable file
Просмотреть файл

@ -0,0 +1,448 @@
#!/bin/bash
# Debug - Break on error
set -e
# Hack: Clear any file locks
sudo rm /var/lib/apt/lists/lock
# Color it
NC='\033[0m' # No Color
RED='\033[0;31m'
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
BLUE='\033[0;34m'
# Time it
scriptstart=$(date +%s)
#
# ROS image for Sawyer development
#
# author="Paul Stubbs (pstubbs@microsoft.com)"
# description="Microsoft Robot Challenge 2018"
# version="1.0"
#
#
#
# Install Bot Framework + Emulator
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nInstall bot framework + Emulator\n***\n***\n***\n***"
echo -e ${NC}
# Time it
start=$(date +%s)
#
#
sudo apt-get update
# install unmet dependencies
sudo apt-get -f install -y
sudo apt-get -f install libindicator7 -y
sudo apt-get -f install libappindicator1 -y
# install any unmet dependencies
sudo apt-get -f install -y
TEMP_DEB="$(mktemp)"
wget -O "$TEMP_DEB" 'https://github.com/Microsoft/BotFramework-Emulator/releases/download/v4.0.15-alpha/botframework-emulator_4.0.15-alpha_amd64.deb'
sudo dpkg -i "$TEMP_DEB"
rm -f "$TEMP_DEB"
# install any unmet dependencies
sudo apt-get -f install -y
sudo apt autoremove -y
# Install Python 3.6
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nInstall Python 3.6\n***\n***\n***\n***"
echo -e ${NC}
sudo add-apt-repository -y ppa:deadsnakes/ppa
sudo apt-get update -y
sudo apt-get install -y python3.6
sudo apt-get install -y python3-pip
# install any unmet dependencies
sudo apt-get -f install -y
python3.6 -m pip install --upgrade pip
# Install Bot Framework Deps
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nInstall bot framework Dependencies\n***\n***\n***\n***"
echo -e ${NC}
python3.6 -m pip install --user aiohttp
python3.6 -m pip install --user requests
python3.6 -m pip install --user botbuilder.schema
python3.6 -m pip install --user botbuilder.core
# Time it
end=$(date +%s)
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
echo -e ${BLUE}
echo -e "Elapsed Time: ${runtime}"
echo -e ${NC}
#
#
#
# Install ROS
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nInstall ROS\n***\n***\n***\n***"
echo -e ${NC}
# Time it
start=$(date +%s)
#
#
# http://sdk.rethinkrobotics.com/intera/Workstation_Setup
# Update to lateset software lists
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nUpdate to lateset software lists\n***\n***\n***\n***"
echo -e ${NC}
# Install some common CLI tools
sudo apt-get update -y
sudo apt-get install -y wget software-properties-common
# Configure Ubuntu repositories. Setup sources.list
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
# Setup keys
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
# Install ROS Kinetic Desktop FUll
echo -e ${GREEN}
echo -e "***\n***\n***\n***\n Install ROS Kinetic Desktop FUll \n***\n***\n***\n***"
echo -e ${NC}
sudo apt-get update -y
sudo apt-get install -y ros-kinetic-desktop-full
# Initialize rosdep
sudo rosdep init || echo -e "${YELLOW}ROSDep Already Exists.${NC}"
rosdep update
# Install rosinstall
sudo apt-get install -y python-rosinstall -y
# Time it
end=$(date +%s)
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
echo -e ${BLUE}
echo -e "Elapsed Time: ${runtime}"
echo -e ${NC}
#
#
#
# Create Development Workspace
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nCreate Development Workspace\n***\n***\n***\n***"
echo -e ${NC}
# Time it
start=$(date +%s)
#
#
#
#Add the path to ROS
echo "source ~/ros_ws/devel/setup.bash" >> ~/.bashrc
# Create ROS Workspace
mkdir -p ~/ros_ws/src
cd ~/ros_ws
source /opt/ros/kinetic/setup.bash
catkin_make
# Time it
end=$(date +%s)
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
echo -e ${BLUE}
echo -e "Elapsed Time: ${runtime}"
echo -e ${NC}
#
#
#
# Install Intera SDK Dependencies
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nInstall Intera SDK Dependencies\n***\n***\n***\n***"
echo -e ${NC}
# Time it
start=$(date +%s)
#
#
#
# Install SDK Dependencies
# Update to lateset software lists
sudo apt-get update
sudo apt-get install -y \
git-core \
python-argparse \
python-wstool \
python-vcstools \
python-rosdep \
ros-kinetic-control-msgs \
ros-kinetic-joystick-drivers \
ros-kinetic-xacro \
ros-kinetic-tf2-ros \
ros-kinetic-rviz \
ros-kinetic-cv-bridge \
ros-kinetic-actionlib \
ros-kinetic-actionlib-msgs \
ros-kinetic-dynamic-reconfigure \
ros-kinetic-trajectory-msgs \
ros-kinetic-rospy-message-converter
# Time it
end=$(date +%s)
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
echo -e ${BLUE}
echo -e "Elapsed Time: ${runtime}"
echo -e ${NC}
#
#
#
# Install Intera Robot SDK
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nInstall Intera Robot SDK\n***\n***\n***\n***"
echo -e ${NC}
# Time it
start=$(date +%s)
#
#
#
cd ~/ros_ws/src
wstool init .
git clone https://github.com/RethinkRobotics/sawyer_robot.git
wstool merge sawyer_robot/sawyer_robot.rosinstall
wstool update
# Source ROS Setup
cd ~/ros_ws
source /opt/ros/kinetic/setup.bash
catkin_make
# Time it
end=$(date +%s)
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
echo -e ${BLUE}
echo -e "Elapsed Time: ${runtime}"
echo -e ${NC}
#
#
#
# Configure Robot Communication/ROS Workspace
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nConfigure Robot Communication/ROS Workspace\n***\n***\n***\n***"
echo -e ${NC}
# Time it
start=$(date +%s)
#
#
#
# Copy the intera.sh script
# The intera.sh file already exists in intera_sdk repo,
# copy the file into your ros workspace.
cp ~/ros_ws/src/intera_sdk/intera.sh ~/ros_ws
# Update the copy of the intera.sh file
# cd ~/ros_ws
# Update ROS Distribution
sed -i 's/ros_version="indigo"/ros_version="kinetic"/' ~/ros_ws/intera.sh
# Update the ROBOTS hostname
sed -i 's/robot_hostname="robot_hostname.local"/robot_hostname="paule.local"/' ~/ros_ws/intera.sh
# TODO:// Need to figure out the docker networking to resolve hostname
# Update YOUR IP or Hostname. This must be resolvable to the Robot
# Choose one. Be sure to add or remove the leading # from the right ones
sed -i 's/your_ip="192.168.XXX.XXX"/your_ip="192.168.XXX.XXX"/' ~/ros_ws/intera.sh
#sed -i 's/#your_hostname="my_computer.local"/your_hostname="my_computer.local"/' intera.sh
# Setup and configure RVIZ
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nSetup and configure RVIZ\n***\n***\n***\n***"
echo -e ${NC}
# TODO:// need to do this still
# Time it
end=$(date +%s)
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
echo -e ${BLUE}
echo -e "Elapsed Time: ${runtime}"
echo -e ${NC}
# http://sdk.rethinkrobotics.com/intera/Gazebo_Tutorial
#
#
#
# Configure Sawyer with Gazebo
#
#
#
# Install Prerequisites
# Update to lateset software lists
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nUpdate to lateset software lists\n***\n***\n***\n***"
echo -e ${NC}
# Time it
start=$(date +%s)
#
#
#
sudo apt-get update -y
sudo apt-get install -y \
gazebo7 \
ros-kinetic-qt-build \
ros-kinetic-gazebo-ros-control \
ros-kinetic-gazebo-ros-pkgs \
ros-kinetic-ros-control \
ros-kinetic-control-toolbox \
ros-kinetic-realtime-tools \
ros-kinetic-ros-controllers \
ros-kinetic-xacro \
python-wstool \
ros-kinetic-tf-conversions \
ros-kinetic-kdl-parser \
ros-kinetic-sns-ik-lib
# Install Sawyer Simulator files
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nInstall Sawyer Simulator files\n***\n***\n***\n***"
echo -e ${NC}
cd ~/ros_ws/src
if [ ! -d src/sawyer_simulator ]
then
# folder does not exist so clone the repo
echo -e ${YELLOW}
echo "~/ros_ws/src/sawyer_simulator folder does not exist, cloning now"
echo -e ${NC}
git clone https://github.com/RethinkRobotics/sawyer_simulator.git
else
# folder does exist so pull to update
echo -e ${YELLOW}
echo "~/ros_ws/src/sawyer_simulator folder already exists, updating now"
echo -e ${NC}
cd ~/ros_ws/src/sawyer_simulator
git pull
cd ~/ros_ws/src
fi
source /opt/ros/kinetic/setup.bash
wstool merge sawyer_simulator/sawyer_simulator.rosinstall
wstool update
# Build the Sources
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nSDK Build the Sources\n***\n***\n***\n***"
echo -e ${NC}
source /opt/ros/kinetic/setup.bash
cd ~/ros_ws
catkin_make
# Time it
end=$(date +%s)
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
echo -e ${BLUE}
echo -e "Elapsed Time: ${runtime}"
echo -e ${NC}
#
#
#
# Install other tools
#
#
#
#
#
#
# Install Chromium Browser
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nInstall Chromium Browser\n***\n***\n***\n***"
echo -e ${NC}
# Time it
start=$(date +%s)
#
#
#
#https://www.linuxbabe.com/ubuntu/install-google-chrome-ubuntu-16-04-lts
sudo su -c "echo 'deb [arch=amd64] http://dl.google.com/linux/chrome/deb/ stable main' >> /etc/apt/sources.list"
TEMP_DEB="$(mktemp)"
wget -O "$TEMP_DEB" 'https://dl.google.com/linux/linux_signing_key.pub'
sudo apt-key add "$TEMP_DEB"
rm -f "$TEMP_DEB"
sudo apt-get update
sudo apt-get install google-chrome-stable
# Time it
end=$(date +%s)
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
echo -e ${BLUE}
echo -e "Elapsed Time: ${runtime}"
echo -e ${NC}
#
#
#
# Install Visual Studio Code
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nInstall Visual Studio Code\n***\n***\n***\n***"
echo -e ${NC}
# Time it
start=$(date +%s)
#
#
#
#https://tecadmin.net/install-visual-studio-code-editor-ubuntu/
sudo su -c "echo 'deb [arch=amd64] http://packages.microsoft.com/repos/vscode stable main' >> /etc/apt/sources.list.d/vscode.list"
curl https://packages.microsoft.com/keys/microsoft.asc | gpg --dearmor > microsoft.gpg
sudo mv microsoft.gpg /etc/apt/trusted.gpg.d/microsoft.gpg
sudo apt-get update
sudo apt-get install code
# Time it
end=$(date +%s)
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
echo -e ${BLUE}
echo -e "Elapsed Time: ${runtime}"
echo -e ${NC}
#
#
#
# Update the machine
echo -e ${GREEN}
echo -e "***\n***\n***\n***\nUpdate the Machine\n***\n***\n***\n***"
echo -e ${NC}
#
#
#
sudo apt-get update
sudo apt-get upgrade -y
sudo apt-get dist-upgrade -y
sudo apt-get autoremove -y
sudo apt-get autoclean -y
# Time it
start=${scriptstart}
end=$(date +%s)
runtime=$(python -c "print '%u:%02u' % ((${end} - ${start})/60, (${end} - ${start})%60)")
echo -e ${BLUE}
echo -e "Total Elapsed Time: ${runtime}"
echo -e ${NC}

595
setup/ubuntu/sawyer_urdf.xml Normal file
Просмотреть файл

@ -0,0 +1,595 @@
<?xml version="1.0" ?><!-- =================================================================================== --><!-- | This document was autogenerated by xacro from ./urdf.xacro | --><!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | --><!-- =================================================================================== --><robot name="sawyer" xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="black">
<color rgba="0 0 0 1"/>
</material>
<material name="darkgray">
<color rgba=".2 .2 .2 1"/>
</material>
<material name="darkred">
<color rgba=".5 .1 .1 1"/>
</material>
<material name="sawyer_red">
<color rgba=".5 .1 .1 1"/>
</material>
<material name="sawyer_gray">
<color rgba="0.75294 0.75294 0.75294 1"/>
</material>
<link name="base"/>
<link name="torso">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0 0 0"/>
</geometry>
<material name="darkgray">
<color rgba=".2 .2 .2 1"/>
</material>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
<mass value="0.0001"/>
<inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
</inertial>
</link>
<link name="pedestal">
<visual>
<origin rpy="1.57079632679 0 -1.57079632679" xyz="0.26 0.345 -0.91488"/>
<geometry>
<mesh filename="package://sawyer_description/meshes/sawyer_pv/pedestal.DAE"/>
</geometry>
<material name="darkgray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="-0.02 0 -0.29"/>
<geometry>
<cylinder length="0.62" radius="0.18"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="60.86397744"/>
<inertia ixx="5.0635929" ixy="0.00103417" ixz="0.80199628" iyy="6.08689388" iyz="0.00105311" izz="4.96191932"/>
</inertial>
</link>
<joint name="torso_t0" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="torso"/>
</joint>
<joint name="pedestal_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<parent link="base"/>
<child link="pedestal"/>
</joint>
<joint name="right_arm_mount" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<parent link="base"/>
<child link="right_arm_base_link"/>
</joint>
<link name="right_arm_base_link">
<inertial>
<origin rpy="0 0 0" xyz="-0.0006241 -2.8025E-05 0.065404"/>
<mass value="2.0687"/>
<inertia ixx="0.0067599" ixy="-4.2024E-05" ixz="-6.1904E-07" iyy="0.0067877" iyz="1.5888E-05" izz="0.0074031"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://sawyer_description/meshes/sawyer_pv/base.DAE"/>
</geometry>
<material name="sawyer_red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.12"/>
<geometry>
<cylinder length="0.24" radius="0.08"/>
</geometry>
</collision>
</link>
<link name="right_l0">
<inertial>
<origin rpy="0 0 0" xyz="0.024366 0.010969 0.14363"/>
<mass value="5.3213"/>
<inertia ixx="0.053314" ixy="0.0047093" ixz="0.011734" iyy="0.057902" iyz="0.0080179" izz="0.023659"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://sawyer_description/meshes/sawyer_mp3/l0.DAE"/>
</geometry>
<material name="sawyer_red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0.08 0 0.23"/>
<geometry>
<sphere radius="0.07"/>
</geometry>
</collision>
</link>
<joint name="right_j0" type="revolute">
<origin rpy="0.0002145211 0.0019869535 -4.784e-06" xyz="0.0005951423 7.50436e-05 0.0797496365"/>
<parent link="right_arm_base_link"/>
<child link="right_l0"/>
<axis xyz="0 0 1"/>
<limit effort="80.0" lower="-3.0503" upper="3.0503" velocity="1.74"/>
</joint>
<link name="head">
<inertial>
<origin rpy="0 0 0" xyz="0.0053207 -2.6549E-05 0.1021"/>
<mass value="1.5795"/>
<inertia ixx="0.011833" ixy="-4.4669E-06" ixz="4.9425E-05" iyy="0.0082709" iyz="4.2124E-07" izz="0.0049661"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://sawyer_description/meshes/sawyer_pv/head.DAE"/>
</geometry>
<material name="sawyer_red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.08"/>
<geometry>
<sphere radius="0.18"/>
</geometry>
</collision>
</link>
<joint name="head_pan" type="revolute">
<origin rpy="-0.000671675137446 -0.000304036251317 0.0961528036545" xyz="2.25041348411e-06 0.000381519444166 0.2965"/>
<parent link="right_l0"/>
<child link="head"/>
<axis xyz="0 0 1"/>
<limit effort="8" lower="-5.0952" upper="0.9064" velocity="1.8"/>
</joint>
<link name="right_torso_itb">
<inertial>
<origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
<mass value="0.0001"/>
<inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
</inertial>
</link>
<joint name="right_torso_itb" type="fixed">
<origin rpy="0 -1.57079632679 0" xyz="-.055 0 .22"/>
<parent link="right_l0"/>
<child link="right_torso_itb"/>
<axis xyz="0 0 1"/>
</joint>
<link name="right_l1">
<inertial>
<origin rpy="0 0 0" xyz="-0.0030849 -0.026811 0.092521"/>
<mass value="4.505"/>
<inertia ixx="0.022398" ixy="-0.00023986" ixz="-0.00029362" iyy="0.014613" iyz="-0.0060875" izz="0.017295"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://sawyer_description/meshes/sawyer_mp3/l1.DAE"/>
</geometry>
<material name="sawyer_red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.1225"/>
<geometry>
<sphere radius="0.07"/>
</geometry>
</collision>
</link>
<joint name="right_j1" type="revolute">
<origin rpy="-1.56975753069 1.57057976739 -0.0010821609" xyz="0.0809015792 0.0498854227 0.237"/>
<parent link="right_l0"/>
<child link="right_l1"/>
<axis xyz="0 0 1"/>
<limit effort="80.0" lower="-3.8095" upper="2.2736" velocity="1.328"/>
</joint>
<link name="right_l2">
<inertial>
<origin rpy="0 0 0" xyz="-0.00016044 -0.014967 0.13582"/>
<mass value="1.745"/>
<inertia ixx="0.025506" ixy="4.4101E-06" ixz="1.4955E-05" iyy="0.0253" iyz="-0.0033204" izz="0.0034179"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://sawyer_description/meshes/sawyer_pv/l2.DAE"/>
</geometry>
<material name="sawyer_red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.08"/>
<geometry>
<cylinder length="0.34" radius="0.06"/>
</geometry>
</collision>
</link>
<joint name="right_j2" type="revolute">
<origin rpy="1.57061011019 -0.000961752 0.0012245467" xyz="-0.0005275357 -0.1400963995 0.1423808458"/>
<parent link="right_l1"/>
<child link="right_l2"/>
<axis xyz="0 0 1"/>
<limit effort="40.0" lower="-3.0426" upper="3.0426" velocity="1.957"/>
</joint>
<link name="right_l3">
<inertial>
<origin rpy="0 0 0" xyz="-0.0048135 -0.0281 -0.084154"/>
<mass value="2.5097"/>
<inertia ixx="0.01016" ixy="-9.7452E-06" ixz="0.00026624" iyy="0.0065685" iyz="0.0030316" izz="0.0069078"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://sawyer_description/meshes/sawyer_pv/l3.DAE"/>
</geometry>
<material name="sawyer_red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 -0.01 -0.12"/>
<geometry>
<sphere radius="0.06"/>
</geometry>
</collision>
</link>
<joint name="right_j3" type="revolute">
<origin rpy="-1.57210568599 0.0005985171 0.0005895877" xyz="-0.0002017802 -0.0423919721 0.2599891702"/>
<parent link="right_l2"/>
<child link="right_l3"/>
<axis xyz="0 0 1"/>
<limit effort="40.0" lower="-3.0439" upper="3.0439" velocity="1.957"/>
</joint>
<link name="right_l4">
<inertial>
<origin rpy="0 0 0" xyz="-0.0018844 0.0069001 0.1341"/>
<mass value="1.1136"/>
<inertia ixx="0.013557" ixy="1.8109E-05" ixz="0.00013523" iyy="0.013555" iyz="0.0010561" izz="0.0013658"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://sawyer_description/meshes/sawyer_pv/l4.DAE"/>
</geometry>
<material name="sawyer_red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.11"/>
<geometry>
<cylinder length="0.30" radius="0.045"/>
</geometry>
</collision>
</link>
<joint name="right_j4" type="revolute">
<origin rpy="1.57046530879 0.0010608855 0.0002608344" xyz="0.0004098916 -0.1249007494 -0.1269097331"/>
<parent link="right_l3"/>
<child link="right_l4"/>
<axis xyz="0 0 1"/>
<limit effort="9.0" lower="-2.9761" upper="2.9761" velocity="3.485"/>
</joint>
<link name="right_arm_itb">
<inertial>
<origin rpy="0 0 0" xyz="0.000000 0.000000 0.000000"/>
<mass value="0.0001"/>
<inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
</inertial>
</link>
<joint name="right_arm_itb" type="fixed">
<origin rpy="0 -1.57079632679 0" xyz="-.055 0 .075"/>
<parent link="right_l4"/>
<child link="right_arm_itb"/>
<axis xyz="0 0 1"/>
</joint>
<link name="right_l5">
<inertial>
<origin rpy="0 0 0" xyz="0.0061133 -0.023697 0.076416"/>
<mass value="1.5625"/>
<inertia ixx="0.0047328" ixy="0.00011526" ixz="4.6269E-05" iyy="0.0029676" iyz="-0.0011557" izz="0.0031762"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://sawyer_description/meshes/sawyer_pv/l5.DAE"/>
</geometry>
<material name="sawyer_red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.1"/>
<geometry>
<sphere radius="0.06"/>
</geometry>
</collision>
</link>
<joint name="right_j5" type="revolute">
<origin rpy="-1.56764146979 0.0008153652 -0.0005184882" xyz="-0.0001775417 0.0308361277 0.2748611106"/>
<parent link="right_l4"/>
<child link="right_l5"/>
<axis xyz="0 0 1"/>
<limit effort="9.0" lower="-2.9761" upper="2.9761" velocity="3.485"/>
</joint>
<link name="right_hand_camera"/>
<joint name="right_hand_camera" type="fixed">
<origin rpy="0.769478294976 1.55880975915 0.764106410692" xyz="0.0379579844756 -0.0325006158013 0.0686967641527"/>
<parent link="right_l5"/>
<child link="right_hand_camera"/>
<axis xyz="0 0 0"/>
</joint>
<link name="right_wrist"/>
<joint name="right_wrist" type="fixed">
<origin rpy="1.57079632679 0 0" xyz="0 0 0.10541"/>
<parent link="right_l5"/>
<child link="right_wrist"/>
<axis xyz="0 0 0"/>
</joint>
<link name="right_l6">
<inertial>
<origin rpy="0 0 0" xyz="-8.0726E-06 0.0085838 -0.0049566"/>
<mass value="0.3292"/>
<inertia ixx="0.00031105" ixy="1.4771E-06" ixz="-3.7074E-07" iyy="0.00021549" iyz="-8.4533E-06" izz="0.00035976"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="package://sawyer_description/meshes/sawyer_mp1/l6.DAE"/>
</geometry>
<material name="sawyer_red"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0.015 -0.01"/>
<geometry>
<cylinder length="0.05" radius="0.055"/>
</geometry>
</collision>
</link>
<joint name="right_j6" type="revolute">
<origin rpy="-1.57238452459 -0.1781653283 3.1424031454" xyz="-2.46636e-05 -0.1098548426 0.1050918016"/>
<parent link="right_l5"/>
<child link="right_l6"/>
<axis xyz="0 0 1"/>
<limit effort="9.0" lower="-4.7124" upper="4.7124" velocity="4.545"/>
</joint>
<link name="right_hand">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.02"/>
<geometry>
<cylinder length="0.03" radius="0.035"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="1.0E-08 1.0E-08 1.0E-08"/>
<mass value="1.0E-08"/>
<inertia ixx="1.0E-08" ixy="1.0E-08" ixz="1.0E-08" iyy="1.0E-08" iyz="1.0E-08" izz="1.0E-08"/>
</inertial>
</link>
<joint name="right_hand" type="fixed">
<origin rpy="0 0 1.570796" xyz="0 0 0.0245"/>
<axis xyz="0 0 1"/>
<parent link="right_l6"/>
<child link="right_hand"/>
</joint>
<link name="right_l1_2">
<inertial>
<origin rpy="0 0 0" xyz="1.0E-08 1.0E-08 1.0E-08"/>
<mass value="1.0E-08"/>
<inertia ixx="1.0E-08" ixy="1.0E-08" ixz="1.0E-08" iyy="1.0E-08" iyz="1.0E-08" izz="1.0E-08"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.035"/>
<geometry>
<cylinder length="0.14" radius="0.07"/>
</geometry>
</collision>
</link>
<joint name="right_j1_2" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="right_l1"/>
<child link="right_l1_2"/>
<axis xyz="0 0 1"/>
</joint>
<link name="right_l2_2">
<inertial>
<origin rpy="0 0 0" xyz="1.0E-08 1.0E-08 1.0E-08"/>
<mass value="1.0E-08"/>
<inertia ixx="1.0E-08" ixy="1.0E-08" ixz="1.0E-08" iyy="1.0E-08" iyz="1.0E-08" izz="1.0E-08"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0 0.26"/>
<geometry>
<sphere radius="0.06"/>
</geometry>
</collision>
</link>
<joint name="right_j2_2" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="right_l2"/>
<child link="right_l2_2"/>
<axis xyz="0 0 1"/>
</joint>
<link name="right_l4_2">
<inertial>
<origin rpy="0 0 0" xyz="1.0E-08 1.0E-08 1.0E-08"/>
<mass value="1.0E-08"/>
<inertia ixx="1.0E-08" ixy="1.0E-08" ixz="1.0E-08" iyy="1.0E-08" iyz="1.0E-08" izz="1.0E-08"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0.01 0.26"/>
<geometry>
<sphere radius="0.06"/>
</geometry>
</collision>
</link>
<joint name="right_j4_2" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="right_l4"/>
<child link="right_l4_2"/>
<axis xyz="0 0 1"/>
</joint>
<link name="screen">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.24 0.14 0.002"/>
</geometry>
<material name="darkgray"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.001"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.0001"/>
<inertia ixx="1e-08" ixy="0" ixz="0" iyy="1e-08" iyz="0" izz="1e-08"/>
</inertial>
</link>
<joint name="display_joint" type="fixed">
<origin rpy="1.57079632679 0 1.57079632679" xyz="0.03 0.0 0.105"/>
<parent link="head"/>
<child link="screen"/>
</joint>
<link name="head_camera"/>
<joint name="head_camera" type="fixed">
<origin rpy="-2.12510633299 -0.019081815459 -1.57079632679" xyz="0.0234787515859 0.00134193139039 0.215168253323"/>
<parent link="head"/>
<child link="head_camera"/>
<axis xyz="0 0 0"/>
</joint>
<link name="controller_box">
<collision>
<origin rpy="0 0 0" xyz="-0.325 0 -0.38"/>
<geometry>
<box size="0.22 0.4 0.53"/>
</geometry>
</collision>
</link>
<joint name="controller_box_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<parent link="base"/>
<child link="controller_box"/>
</joint>
<link name="pedestal_feet">
<collision>
<origin rpy="0 0 0" xyz="-0.1225 0 -0.758"/>
<geometry>
<box size="0.77 0.7 0.31"/>
</geometry>
</collision>
</link>
<joint name="pedestal_feet_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
<parent link="base"/>
<child link="pedestal_feet"/>
</joint>
<joint name="stp_021804TP00031_base_joint" type="fixed">
<child link="stp_021804TP00031_base">
</child>
<origin rpy="0 0 0" xyz="0 0 -0.005">
</origin>
<parent link="right_hand">
</parent>
</joint>
<joint name="stp_021804TP00031_tip_1_joint" type="fixed">
<child link="stp_021804TP00031_tip_1">
</child>
<origin rpy="0 0 0" xyz="0 0 0">
</origin>
<parent link="stp_021804TP00031_base">
</parent>
</joint>
<joint name="stp_021804TP00031_tip_2_joint" type="fixed">
<child link="stp_021804TP00031_tip_2">
</child>
<origin rpy="0 0 0" xyz="0 0 0">
</origin>
<parent link="stp_021804TP00031_base">
</parent>
</joint>
<link name="stp_021804TP00031_base">
<collision>
<geometry>
<cylinder length="0.0269" radius="0.070">
</cylinder>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.01845">
</origin>
</collision>
<inertial>
<inertia ixx="0.00016416" ixy="0" ixz="0" iyy="0.00025526" iyz="0" izz="0.00035964">
</inertia>
<mass value="0.31382">
</mass>
<origin rpy="0 0 0" xyz="0.0085294 0 0.015587">
</origin>
</inertial>
<visual>
<geometry>
<mesh filename="package://intera_tools_description/meshes/smart_tool_plate/smart_tool_plate.DAE">
</mesh>
</geometry>
<material name="">
<color rgba="0 0 0 1">
</color>
</material>
<origin rpy="0 0 0" xyz="0 0 0">
</origin>
</visual>
</link>
<link name="stp_021804TP00031_tip_1">
<collision>
<geometry>
<box size="0.01 0.01 0.01">
</box>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0">
</origin>
</collision>
<inertial>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0">
</inertia>
<mass value="0">
</mass>
<origin rpy="0 0 0" xyz="0 0 0">
</origin>
</inertial>
<visual>
<geometry>
<box size="0.015 0.015 0.015">
</box>
</geometry>
<material name="darkgrey">
<color rgba="0.2 0.3 0.3 1">
</color>
</material>
<origin rpy="0 0 0" xyz="0 0 0">
</origin>
</visual>
</link>
<link name="stp_021804TP00031_tip_2">
<collision>
<geometry>
<box size="0.01 0.01 0.01">
</box>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0">
</origin>
</collision>
<inertial>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0">
</inertia>
<mass value="0">
</mass>
<origin rpy="0 0 0" xyz="0 0 0">
</origin>
</inertial>
<visual>
<geometry>
<box size="0.015 0.015 0.015">
</box>
</geometry>
<material name="darkgrey">
<color rgba="0.2 0.3 0.3 1">
</color>
</material>
<origin rpy="0 0 0" xyz="0 0 0">
</origin>
</visual>
</link>
</robot>

23367
setup/ubuntu/subprocess Normal file
Просмотреть файл

Разница между файлами не показана из-за своего большого размера Загрузить разницу

62
setup/ubuntu/talk-to-my-robot-node.py Executable file
Просмотреть файл

@ -0,0 +1,62 @@
#!/usr/bin/env python
import json
# rospy - ROS Python API
import rospy
# intera_interface - Sawyer Python API
import intera_interface
# initialize our ROS node, registering it with the Master
rospy.init_node('Hello_Sawyer')
# create an instance of intera_interface's Limb class
limb = intera_interface.Limb('right')
# get the right limb's current joint angles
angles = limb.joint_angles()
# print the current joint angles as valid json
print json.dumps(angles)
# move to neutral pose
limb.move_to_neutral()
# get the right limb's current joint angles now that it is in neutral
angles = limb.joint_angles()
# print the current joint angles again as valid json
print json.dumps(angles)
# reassign new joint angles (all zeros) which we will later command to the limb
angles['right_j0']=0.0
angles['right_j1']=0.0
angles['right_j2']=0.0
angles['right_j3']=0.0
angles['right_j4']=0.0
angles['right_j5']=0.0
angles['right_j6']=0.0
# print the joint angle command as valid json
print json.dumps(angles)
# move the right arm to those joint angles
limb.move_to_joint_positions(angles)
# Sawyer wants to say hello, let's wave the arm
# store the first wave position
wave_1 = {'right_j6': -1.5126, 'right_j5': -0.3438, 'right_j4': 1.5126, 'right_j3': -1.3833, 'right_j2': 0.03726, 'right_j1': 0.3526, 'right_j0': -0.4259}
# store the second wave position
wave_2 = {'right_j6': -1.5101, 'right_j5': -0.3806, 'right_j4': 1.5103, 'right_j3': -1.4038, 'right_j2': -0.2609, 'right_j1': 0.3940, 'right_j0': -0.4281}
# wave three times
for _move in range(3):
limb.move_to_joint_positions(wave_1)
rospy.sleep(0.5)
limb.move_to_joint_positions(wave_2)
rospy.sleep(0.5)

136
setup/ubuntu/talk-to-my-robot.py Executable file
Просмотреть файл

@ -0,0 +1,136 @@
import json
import requests
# Used to call Python 2.7 from 3.6
import subprocess
from aiohttp import web
from botbuilder.core import (BotFrameworkAdapter, BotFrameworkAdapterSettings, TurnContext)
from botbuilder.schema import (Activity, ActivityTypes)
BOT_APP_ID = ''
BOT_APP_PASSWORD = ''
LUIS_APP_ID = '357b6036-c961-4bb0-9aa9-bbbcdf1e32f0'
LUIS_SUBSCRIPTION_KEY = 'b7f33ef452a544079fb86356479f9300'
SETTINGS = BotFrameworkAdapterSettings(BOT_APP_ID, BOT_APP_PASSWORD)
ADAPTER = BotFrameworkAdapter(SETTINGS)
class LuisApiService:
@staticmethod
def postUtterance(message):
params ={
# Query parameter
'q': message,
# Optional request parameters, set to default values
'timezoneOffset': '0',
'verbose': 'false',
'spellCheck': 'false',
'staging': 'false',
}
headers = {
# Request headers
'Ocp-Apim-Subscription-Key': LUIS_SUBSCRIPTION_KEY,
}
try:
r = requests.get('https://westus.api.cognitive.microsoft.com/luis/v2.0/apps/%s' % LUIS_APP_ID, headers=headers, params=params)
topScoreIntent = r.json()['topScoringIntent']
intent = topScoreIntent['intent'] if topScoreIntent['score'] > 0.5 else 'None'
return intent
except Exception as e:
print("[Errno {0}] {1}".format(e.errno, e.strerror))
class BotRequestHandler:
async def create_reply_activity(request_activity, text) -> Activity:
return Activity(
type=ActivityTypes.message,
channel_id=request_activity.channel_id,
conversation=request_activity.conversation,
recipient=request_activity.from_property,
from_property=request_activity.recipient,
text=text,
service_url=request_activity.service_url)
async def handle_message(context: TurnContext) -> web.Response:
# Access the state for the conversation between the user and the bot.
intent = LuisApiService.postUtterance(context.activity.text)
response = await BotRequestHandler.create_reply_activity(context.activity, f'Top Intent: {intent}.')
await context.send_activity(response)
if intent == 'ShowStats':
BotCommandHandler.show_stats()
elif intent == 'MoveArm':
BotCommandHandler.move_arm()
else:
print('Please provide a valid instruction')
return web.Response(status=202)
async def handle_conversation_update(context: TurnContext) -> web.Response:
if context.activity.members_added[0].id != context.activity.recipient.id:
response = await BotRequestHandler.create_reply_activity(context.activity, 'Welcome to Sawyer Robot!')
await context.send_activity(response)
return web.Response(status=200)
async def unhandled_activity() -> web.Response:
return web.Response(status=404)
@staticmethod
async def request_handler(context: TurnContext) -> web.Response:
if context.activity.type == 'message':
return await BotRequestHandler.handle_message(context)
elif context.activity.type == 'conversationUpdate':
return await BotRequestHandler.handle_conversation_update(context)
else:
return await BotRequestHandler.unhandled_activity()
async def messages(req: web.web_request) -> web.Response:
body = await req.json()
activity = Activity().deserialize(body)
auth_header = req.headers['Authorization'] if 'Authorization' in req.headers else ''
try:
return await ADAPTER.process_activity(activity, auth_header, BotRequestHandler.request_handler)
except Exception as e:
raise e
class BotCommandHandler:
def move_arm():
print('Moving arm... do something cool')
# launch your python2 script using bash
python3_command = "python2.7 talk-to-my-robot-node.py"
process = subprocess.Popen(python3_command.split(), stdout=subprocess.PIPE)
output, error = process.communicate() # receive output from the python2 script
print('done moving . . .')
print('returncode:' + str(process.returncode))
print('output:' + output.decode("utf-8"))
def show_stats():
print('Showing stats... do something')
# launch your python2 script using bash
python3_command = "python2.7 bot-stats-node.py"
process = subprocess.Popen(python3_command.split(), stdout=subprocess.PIPE)
output, error = process.communicate() # receive output from the python2 script
print('done getting state . . .')
print('returncode:' + str(process.returncode))
print('output:' + output.decode("utf-8"))
app = web.Application()
app.router.add_post('/api/messages', BotRequestHandler.messages)
try:
web.run_app(app, host='localhost', port=9000)
print('Started http server on localhost:9000')
except Exception as e:
raise e

42
src/sorting_demo/CMakeLists.txt Normal file
Просмотреть файл

@ -0,0 +1,42 @@
cmake_minimum_required(VERSION 2.8.3)
project(sorting_demo)
find_package(catkin REQUIRED COMPONENTS
cv_bridge
rospy
actionlib
sensor_msgs
std_msgs
control_msgs
trajectory_msgs
dynamic_reconfigure
intera_core_msgs
intera_motion_msgs
gazebo_msgs
)
catkin_python_setup()
catkin_package(
CATKIN_DEPENDS
rospy
intera_core_msgs
gazebo_msgs
)
#############
## Install ##
#############
install(PROGRAMS
src/sorting_demo/program.py
scripts/test_moveit.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
foreach(dir launch models)
install(DIRECTORY ${dir}/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
endforeach(dir)

546
src/sorting_demo/config/rviz.rviz Normal file
Просмотреть файл

@ -0,0 +1,546 @@
Panels:
- Class: rviz/Displays
Help Height: 0
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /TF1
- /Path1
- /Path2
- /RobotModel1
Splitter Ratio: 0.5
Tree Height: 573
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: Image
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
base:
Value: true
block0::block:
Value: true
block1::block:
Value: true
block2::block:
Value: true
block3::block:
Value: true
block4::block:
Value: true
block5::block:
Value: true
block6::block:
Value: true
block7::block:
Value: true
block8::block:
Value: true
controller_box:
Value: true
head:
Value: true
head_camera:
Value: true
head_camera_optical:
Value: true
pedestal:
Value: true
pedestal_feet:
Value: true
right_arm_base_link:
Value: true
right_arm_itb:
Value: true
right_connector_plate_base:
Value: true
right_connector_plate_mount:
Value: true
right_electric_gripper_base:
Value: true
right_gripper_base:
Value: true
right_gripper_l_finger:
Value: true
right_gripper_l_finger_tip:
Value: true
right_gripper_r_finger:
Value: true
right_gripper_r_finger_tip:
Value: true
right_gripper_tip:
Value: true
right_hand:
Value: true
right_hand_camera:
Value: true
right_hand_camera_optical:
Value: true
right_l0:
Value: true
right_l1:
Value: true
right_l1_2:
Value: true
right_l2:
Value: true
right_l2_2:
Value: true
right_l3:
Value: true
right_l4:
Value: true
right_l4_2:
Value: true
right_l5:
Value: true
right_l6:
Value: true
right_torso_itb:
Value: true
right_wrist:
Value: true
screen:
Value: true
torso:
Value: true
tray0::tray_base:
Value: true
tray1::tray_base:
Value: true
tray2::tray_base:
Value: true
world:
Value: true
Marker Scale: 0.25
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
world:
base:
controller_box:
{}
pedestal:
{}
pedestal_feet:
{}
right_arm_base_link:
right_l0:
head:
head_camera:
head_camera_optical:
{}
screen:
{}
right_l1:
right_l1_2:
{}
right_l2:
right_l2_2:
{}
right_l3:
right_l4:
right_arm_itb:
{}
right_l4_2:
{}
right_l5:
right_hand_camera:
right_hand_camera_optical:
{}
right_l6:
right_hand:
right_gripper_base:
right_connector_plate_base:
right_connector_plate_mount:
right_electric_gripper_base:
right_gripper_l_finger:
right_gripper_l_finger_tip:
{}
right_gripper_r_finger:
right_gripper_r_finger_tip:
{}
right_gripper_tip:
{}
right_wrist:
{}
right_torso_itb:
{}
torso:
{}
block0::block:
{}
block1::block:
{}
block2::block:
{}
block3::block:
{}
block4::block:
{}
block5::block:
{}
block6::block:
{}
block7::block:
{}
block8::block:
{}
tray0::tray_base:
{}
tray1::tray_base:
{}
tray2::tray_base:
{}
Update Interval: 0
Value: true
- Class: rviz/Image
Enabled: true
Image Topic: /io/internal_camera/right_hand_camera/image_rect
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 1
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /reelrbtx_follow_controller/reel_mouth_path
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 0; 0; 255
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /reelrbtx_follow_controller/reel_base_path
Unreliable: false
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base:
Alpha: 1
Show Axes: false
Show Trail: false
controller_box:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_camera:
Alpha: 1
Show Axes: false
Show Trail: false
head_camera_optical:
Alpha: 1
Show Axes: false
Show Trail: false
pedestal:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
pedestal_feet:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_arm_base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_arm_itb:
Alpha: 1
Show Axes: false
Show Trail: false
right_connector_plate_base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_connector_plate_mount:
Alpha: 1
Show Axes: false
Show Trail: false
right_electric_gripper_base:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_gripper_base:
Alpha: 1
Show Axes: false
Show Trail: false
right_gripper_l_finger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_gripper_l_finger_tip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_gripper_r_finger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_gripper_r_finger_tip:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_gripper_tip:
Alpha: 1
Show Axes: false
Show Trail: false
right_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_hand_camera:
Alpha: 1
Show Axes: false
Show Trail: false
right_hand_camera_optical:
Alpha: 1
Show Axes: false
Show Trail: false
right_l0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_l1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_l1_2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_l2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_l2_2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_l3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_l4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_l4_2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_l5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_l6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_torso_itb:
Alpha: 1
Show Axes: false
Show Trail: false
right_wrist:
Alpha: 1
Show Axes: false
Show Trail: false
screen:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
torso:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
world:
Alpha: 1
Show Axes: false
Show Trail: false
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 2.20854378
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.463854343
Y: 0.0451556407
Z: 0.0798519328
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.304797798
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 5.46816921
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1056
Hide Left Dock: false
Hide Right Dock: false
Image:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000400000000000002f400000396fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000027e000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001d3000001300000000000000000fb0000000a0049006d00610067006501000002ac000001120000001600ffffff000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003efc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003300000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1855
X: 65
Y: 24

18
src/sorting_demo/launch/robot_sorting_demo_control.launch Normal file
Просмотреть файл

@ -0,0 +1,18 @@
<launch>
<arg name="electric_gripper" default="false"/>
<!-- Start the Sawyer pick and place demo -->
<node pkg="sorting_demo" type="program.py" name="sorting_demo" >
<remap from="/joint_states" to="/robot/joint_states"/>
</node>
<node pkg="topic_tools" type="relay" args="/robot/joint_states /joint_states" name="statedup"/>
<node pkg="sawyer_ik_5d" type="sawyer_ik_5d_node" name="sawyer_ik_5d_node" output="screen"/>
<node pkg="intera_interface" type="joint_trajectory_action_server.py" name="trajectory_action_server"/>
<node pkg="sorting_demo" type="gripper_action_server.py" name="gripper_action_server"/>
<include file="$(find sawyer_moveit_config)/launch/sawyer_moveit.launch">
<arg name="electric_gripper" value="$(arg electric_gripper)"/>
</include>
</launch>

9
src/sorting_demo/launch/sorting_demo.launch Normal file
Просмотреть файл

@ -0,0 +1,9 @@
<launch>
<include file="$(find sawyer_gazebo)/launch/sawyer_world.launch">
<arg name="electric_gripper" value="true"/>
</include>
<include file="$(find sorting_demo)/launch/robot_sorting_demo_control.launch">
<arg name="electric_gripper" value="true"/>
</include>
</launch>

47
src/sorting_demo/models/block/block.urdf.xacro Normal file
Просмотреть файл

@ -0,0 +1,47 @@
<robot xmlns:xacro="http://ros.org/wiki/xacro"
name="block">
<xacro:arg name="material" default="Gazebo/Black" />
<xacro:arg name="edge_length" default="0.04" />
<xacro:property name="block_density" value="3900" />
<link name="block">
<inertial>
<mass value="${block_density * $(arg edge_length)**3}" />
<inertia ixx="${(1/12) * block_density * $(arg edge_length)**3 * (2 * $(arg edge_length)**2)}" ixy="0.0" ixz="0.0"
iyy="${(1/12) * block_density * $(arg edge_length)**3 * (2 * $(arg edge_length)**2)}" iyz="0.0"
izz="${(1/12) * block_density * $(arg edge_length)**3 * (2 * $(arg edge_length)**2)}" />
</inertial>
<visual>
<geometry>
<box size="$(arg edge_length) $(arg edge_length) $(arg edge_length)" />
</geometry>
</visual>
<collision>
<geometry>
<box size="$(arg edge_length) $(arg edge_length) $(arg edge_length)" />
</geometry>
</collision>
</link>
<gazebo reference="block">
<material>$(arg material)</material>
<mu1>10000</mu1>
<mu2>10000</mu2>
<collision>
<surface>
<friction>
<torsional>
<coefficient>0.456</coefficient>
<use_patch_radius>true</use_patch_radius>
<patch_radius>0.05</patch_radius>
</torsional>
</friction>
</surface>
</collision>
</gazebo>
</robot>

Двоичные данные
src/sorting_demo/models/table/materials/textures/Maple.jpg Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 36 KiB

Двоичные данные
src/sorting_demo/models/table/materials/textures/Maple_dark.jpg Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 21 KiB

Двоичные данные
src/sorting_demo/models/table/materials/textures/Wood_Floor_Dark.jpg Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 44 KiB

307
src/sorting_demo/models/table/meshes/cafe_table.dae Normal file
Просмотреть файл

@ -0,0 +1,307 @@
<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
<asset>
<contributor>
<authoring_tool>SketchUp 14.1.1282</authoring_tool>
</contributor>
<created>2014-12-10T16:57:23Z</created>
<modified>2014-12-10T16:57:23Z</modified>
<unit meter="0.0254" name="inch" />
<up_axis>Z_UP</up_axis>
</asset>
<library_visual_scenes>
<visual_scene id="ID1">
<node name="SketchUp">
<instance_geometry url="#ID38">
<bind_material>
<technique_common>
<instance_material symbol="Material2" target="#ID39">
<bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
</instance_material>
</technique_common>
</bind_material>
</instance_geometry>
<node id="ID2" name="Top_Square">
<matrix>1 0 0 1.024181e-014 0 1 0 8.881784e-015 0 0 1 29 0 0 0 1</matrix>
<instance_node url="#ID3" />
</node>
</node>
</visual_scene>
</library_visual_scenes>
<library_nodes>
<node id="ID3" name="Top_Square">
<instance_geometry url="#ID4">
<bind_material>
<technique_common>
<instance_material symbol="Material2" target="#ID5">
<bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
</instance_material>
<instance_material symbol="Material3" target="#ID14">
<bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
</instance_material>
</technique_common>
</bind_material>
</instance_geometry>
<instance_geometry url="#ID22">
<bind_material>
<technique_common>
<instance_material symbol="Material2" target="#ID14">
<bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
</instance_material>
</technique_common>
</bind_material>
</instance_geometry>
<instance_geometry url="#ID30">
<bind_material>
<technique_common>
<instance_material symbol="Material2" target="#ID14">
<bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
</instance_material>
</technique_common>
</bind_material>
</instance_geometry>
</node>
</library_nodes>
<library_geometries>
<geometry id="ID4">
<mesh>
<source id="ID10">
<float_array id="ID19" count="96">18 18 0 18 -18 1.5 18 -18 0 18 18 1.5 18 18 1.5 18 18 0 18 -18 1.5 18 -18 0 -18 18 1.5 18 18 0 -18 18 0 18 18 1.5 18 18 1.5 -18 18 1.5 18 18 0 -18 18 0 18 -18 1.5 -18 -18 0 18 -18 0 -18 -18 1.5 -18 -18 1.5 18 -18 1.5 -18 -18 0 18 -18 0 -18 18 1.5 -18 -18 0 -18 -18 1.5 -18 18 0 -18 18 0 -18 18 1.5 -18 -18 0 -18 -18 1.5</float_array>
<technique_common>
<accessor count="32" source="#ID19" stride="3">
<param name="X" type="float" />
<param name="Y" type="float" />
<param name="Z" type="float" />
</accessor>
</technique_common>
</source>
<source id="ID11">
<float_array id="ID20" count="96">1 0 0 1 0 0 1 0 0 1 0 0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -0 1 -0 -0 1 -0 -0 1 -0 -0 1 -0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 -0 1 -0 -0 1 -0 -0 1 -0 -0 1 -0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 1 -0 -0 1 -0 -0 1 -0 -0 1 -0 -0</float_array>
<technique_common>
<accessor count="32" source="#ID20" stride="3">
<param name="X" type="float" />
<param name="Y" type="float" />
<param name="Z" type="float" />
</accessor>
</technique_common>
</source>
<source id="ID13">
<float_array id="ID21" count="64">0.15 0 -0.15 1.5 -0.15 0 0.15 1.5 0.75 0.0625 0.75 0 -0.75 0.0625 -0.75 0 0.15 1.5 -0.15 0 0.15 0 -0.15 1.5 -0.75 0.0625 0.75 0.0625 -0.75 0 0.75 0 0.15 1.5 -0.15 0 0.15 0 -0.15 1.5 -0.75 0.0625 0.75 0.0625 -0.75 0 0.75 0 -0.15 1.5 0.15 0 0.15 1.5 -0.15 0 -0.75 0 -0.75 0.0625 0.75 0 0.75 0.0625</float_array>
<technique_common>
<accessor count="32" source="#ID21" stride="2">
<param name="S" type="float" />
<param name="T" type="float" />
</accessor>
</technique_common>
</source>
<vertices id="ID12">
<input semantic="POSITION" source="#ID10" />
<input semantic="NORMAL" source="#ID11" />
</vertices>
<triangles count="8" material="Material2">
<input offset="0" semantic="VERTEX" source="#ID12" />
<input offset="1" semantic="TEXCOORD" source="#ID13" />
<p>0 0 1 1 2 2 1 1 0 0 3 3 8 8 9 9 10 10 9 9 8 8 11 11 16 16 17 17 18 18 17 17 16 16 19 19 24 24 25 25 26 26 25 25 24 24 27 27</p>
</triangles>
<triangles count="8" material="Material3">
<input offset="0" semantic="VERTEX" source="#ID12" />
<input offset="1" semantic="TEXCOORD" source="#ID13" />
<p>4 4 5 5 6 6 7 7 6 6 5 5 12 12 13 13 14 14 15 15 14 14 13 13 20 20 21 21 22 22 23 23 22 22 21 21 28 28 29 29 30 30 31 31 30 30 29 29</p>
</triangles>
</mesh>
</geometry>
<geometry id="ID22">
<mesh>
<source id="ID23">
<float_array id="ID27" count="24">18 -18 1.5 -18 18 1.5 -18 -18 1.5 18 18 1.5 18 18 1.5 18 -18 1.5 -18 18 1.5 -18 -18 1.5</float_array>
<technique_common>
<accessor count="8" source="#ID27" stride="3">
<param name="X" type="float" />
<param name="Y" type="float" />
<param name="Z" type="float" />
</accessor>
</technique_common>
</source>
<source id="ID24">
<float_array id="ID28" count="24">0 0 1 0 0 1 0 0 1 0 0 1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1</float_array>
<technique_common>
<accessor count="8" source="#ID28" stride="3">
<param name="X" type="float" />
<param name="Y" type="float" />
<param name="Z" type="float" />
</accessor>
</technique_common>
</source>
<source id="ID26">
<float_array id="ID29" count="8">0.75 -0.75 -0.75 0.75 -0.75 -0.75 0.75 0.75</float_array>
<technique_common>
<accessor count="4" source="#ID29" stride="2">
<param name="S" type="float" />
<param name="T" type="float" />
</accessor>
</technique_common>
</source>
<vertices id="ID25">
<input semantic="POSITION" source="#ID23" />
<input semantic="NORMAL" source="#ID24" />
</vertices>
<triangles count="4" material="Material2">
<input offset="0" semantic="VERTEX" source="#ID25" />
<input offset="1" semantic="TEXCOORD" source="#ID26" />
<p>0 0 1 1 2 2 1 1 0 0 3 3 4 3 5 0 6 1 7 2 6 1 5 0</p>
</triangles>
</mesh>
</geometry>
<geometry id="ID30">
<mesh>
<source id="ID31">
<float_array id="ID35" count="24">18 18 0 -18 -18 0 -18 18 0 18 -18 0 18 -18 0 18 18 0 -18 -18 0 -18 18 0</float_array>
<technique_common>
<accessor count="8" source="#ID35" stride="3">
<param name="X" type="float" />
<param name="Y" type="float" />
<param name="Z" type="float" />
</accessor>
</technique_common>
</source>
<source id="ID32">
<float_array id="ID36" count="24">0 0 -1 0 0 -1 0 0 -1 0 0 -1 -0 -0 1 -0 -0 1 -0 -0 1 -0 -0 1</float_array>
<technique_common>
<accessor count="8" source="#ID36" stride="3">
<param name="X" type="float" />
<param name="Y" type="float" />
<param name="Z" type="float" />
</accessor>
</technique_common>
</source>
<source id="ID34">
<float_array id="ID37" count="8">-0.75 0.75 0.75 -0.75 0.75 0.75 -0.75 -0.75</float_array>
<technique_common>
<accessor count="4" source="#ID37" stride="2">
<param name="S" type="float" />
<param name="T" type="float" />
</accessor>
</technique_common>
</source>
<vertices id="ID33">
<input semantic="POSITION" source="#ID31" />
<input semantic="NORMAL" source="#ID32" />
</vertices>
<triangles count="4" material="Material2">
<input offset="0" semantic="VERTEX" source="#ID33" />
<input offset="1" semantic="TEXCOORD" source="#ID34" />
<p>0 0 1 1 2 2 1 1 0 0 3 3 4 3 5 0 6 1 7 2 6 1 5 0</p>
</triangles>
</mesh>
</geometry>
<geometry id="ID38">
<mesh>
<source id="ID41">
<float_array id="ID44" count="240">0.8181818 0.8181818 2 0.8181818 -0.8181818 29 0.8181818 -0.8181818 2 0.8181818 0.8181818 29 0.8181818 0.8181818 29 0.8181818 0.8181818 2 0.8181818 -0.8181818 29 0.8181818 -0.8181818 2 11 -11 -5.53967e-018 0.8181818 0.8181818 2 0.8181818 -0.8181818 2 11 11 -5.671108e-018 11 11 -5.671108e-018 11 -11 -5.53967e-018 0.8181818 0.8181818 2 0.8181818 -0.8181818 2 -0.8181818 0.8181818 29 0.8181818 0.8181818 2 -0.8181818 0.8181818 2 0.8181818 0.8181818 29 0.8181818 0.8181818 29 -0.8181818 0.8181818 29 0.8181818 0.8181818 2 -0.8181818 0.8181818 2 0.8181818 -0.8181818 29 -0.8181818 0.8181818 29 -0.8181818 -0.8181818 29 0.8181818 0.8181818 29 0.8181818 0.8181818 29 0.8181818 -0.8181818 29 -0.8181818 0.8181818 29 -0.8181818 -0.8181818 29 0.8181818 -0.8181818 29 -0.8181818 -0.8181818 2 0.8181818 -0.8181818 2 -0.8181818 -0.8181818 29 -0.8181818 -0.8181818 29 0.8181818 -0.8181818 29 -0.8181818 -0.8181818 2 0.8181818 -0.8181818 2 11 11 -5.671108e-018 11 -11 -5.53967e-018 0 0 0 0 0 0 11 -11 -5.53967e-018 11 11 -5.671108e-018 -0.8181818 0.8181818 2 -11 11 5.53967e-018 -0.8181818 0.8181818 2 -11 11 5.53967e-018 -0.8181818 -0.8181818 2 -11 -11 5.671108e-018 -0.8181818 -0.8181818 2 -11 -11 5.671108e-018 -0.8181818 0.8181818 29 -0.8181818 -0.8181818 2 -0.8181818 -0.8181818 29 -0.8181818 0.8181818 2 -0.8181818 0.8181818 2 -0.8181818 0.8181818 29 -0.8181818 -0.8181818 2 -0.8181818 -0.8181818 29 11 -11 -5.53967e-018 -11 -11 5.671108e-018 0 0 0 0 0 0 -11 -11 5.671108e-018 11 -11 -5.53967e-018 11 11 -5.671108e-018 0 0 0 -11 11 5.53967e-018 -11 11 5.53967e-018 0 0 0 11 11 -5.671108e-018 0 0 0 -11 -11 5.671108e-018 -11 11 5.53967e-018 -11 11 5.53967e-018 -11 -11 5.671108e-018 0 0 0</float_array>
<technique_common>
<accessor count="80" source="#ID44" stride="3">
<param name="X" type="float" />
<param name="Y" type="float" />
<param name="Z" type="float" />
</accessor>
</technique_common>
</source>
<source id="ID42">
<float_array id="ID45" count="240">1 0 0 1 0 0 1 0 0 1 0 0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 0.0972804 -0.0972804 0.9904913 0.0972804 0.0972804 0.9904913 0.0972804 -0.0972804 0.9904913 0.0972804 0.0972804 0.9904913 -0.0972804 -0.0972804 -0.9904913 -0.0972804 0.0972804 -0.9904913 -0.0972804 -0.0972804 -0.9904913 -0.0972804 0.0972804 -0.9904913 -0 1 -0 -0 1 -0 -0 1 -0 -0 1 -0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 1 0 0 1 0 0 1 0 0 1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 0 -1 0 0 -1 0 0 -1 0 0 -1 0 -0 1 -0 -0 1 -0 -0 1 -0 -0 1 -0 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1 -0.0972804 0.0972804 0.9904913 -0.0972804 0.0972804 0.9904913 0.0972804 -0.0972804 -0.9904913 0.0972804 -0.0972804 -0.9904913 -0.0972804 -0.0972804 0.9904913 -0.0972804 -0.0972804 0.9904913 0.0972804 0.0972804 -0.9904913 0.0972804 0.0972804 -0.9904913 -1 0 0 -1 0 0 -1 0 0 -1 0 0 1 -0 -0 1 -0 -0 1 -0 -0 1 -0 -0 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 -5.095808e-019 -5.974465e-021 -1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1 5.095808e-019 5.974465e-021 1</float_array>
<technique_common>
<accessor count="80" source="#ID45" stride="3">
<param name="X" type="float" />
<param name="Y" type="float" />
<param name="Z" type="float" />
</accessor>
</technique_common>
</source>
<vertices id="ID43">
<input semantic="POSITION" source="#ID41" />
<input semantic="NORMAL" source="#ID42" />
</vertices>
<triangles count="44" material="Material2">
<input offset="0" semantic="VERTEX" source="#ID43" />
<p>0 1 2 1 0 3 4 5 6 7 6 5 8 9 10 9 8 11 12 13 14 15 14 13 16 17 18 17 16 19 20 21 22 23 22 21 24 25 26 25 24 27 28 29 30 31 30 29 32 33 34 33 32 35 36 37 38 39 38 37 40 41 42 43 44 45 46 11 47 11 46 9 14 48 12 49 12 48 8 50 51 50 8 10 15 13 52 53 52 13 54 55 56 55 54 57 58 59 60 61 60 59 62 63 64 65 66 67 68 69 70 71 72 73 50 47 51 47 50 46 48 52 49 53 49 52 74 75 76 77 78 79</p>
</triangles>
</mesh>
</geometry>
</library_geometries>
<library_materials>
<material id="ID5" name="Wood_Floor_Dark">
<instance_effect url="#ID6" />
</material>
<material id="ID14" name="Maple">
<instance_effect url="#ID15" />
</material>
<material id="ID39" name="Blue1">
<instance_effect url="#ID40" />
</material>
</library_materials>
<library_effects>
<effect id="ID6">
<profile_COMMON>
<newparam sid="ID8">
<surface type="2D">
<init_from>ID7</init_from>
</surface>
</newparam>
<newparam sid="ID9">
<sampler2D>
<source>ID8</source>
</sampler2D>
</newparam>
<technique sid="COMMON">
<lambert>
<diffuse>
<texture texture="ID9" texcoord="UVSET0" />
</diffuse>
</lambert>
</technique>
</profile_COMMON>
</effect>
<effect id="ID15">
<profile_COMMON>
<newparam sid="ID17">
<surface type="2D">
<init_from>ID16</init_from>
</surface>
</newparam>
<newparam sid="ID18">
<sampler2D>
<source>ID17</source>
</sampler2D>
</newparam>
<technique sid="COMMON">
<lambert>
<diffuse>
<texture texture="ID18" texcoord="UVSET0" />
</diffuse>
</lambert>
</technique>
</profile_COMMON>
</effect>
<effect id="ID40">
<profile_COMMON>
<technique sid="COMMON">
<lambert>
<diffuse>
<color>0.3019608 0.4235294 0.5411765 1</color>
</diffuse>
</lambert>
</technique>
</profile_COMMON>
</effect>
</library_effects>
<library_images>
<image id="ID7">
<init_from>Wood_Floor_Dark.jpg</init_from>
</image>
<image id="ID16">
<init_from>Maple_dark.jpg</init_from>
</image>
</library_images>
<scene>
<instance_visual_scene url="#ID1" />
</scene>
</COLLADA>

18
src/sorting_demo/models/table/model.config Normal file
Просмотреть файл

@ -0,0 +1,18 @@
<?xml version="1.0"?>
<model>
<name>Cafe table</name>
<version>1.0</version>
<sdf version="1.4">model-1_4.sdf</sdf>
<sdf version="1.5">model.sdf</sdf>
<author>
<name>Nate Koenig</name>
<email>nate@osrfoundation.org</email>
</author>
<description>
A model of a cafe table.
</description>
</model>

52
src/sorting_demo/models/table/model.sdf Normal file
Просмотреть файл

@ -0,0 +1,52 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<model name="cafe_table">
<static>true</static>
<link name="link">
<collision name="surface">
<pose>0 0 0.755 0 0 0</pose>
<geometry>
<box>
<size>0.913 0.913 0.04</size>
</box>
</geometry>
</collision>
<collision name="column">
<pose>0 0 0.37 0 0 0</pose>
<geometry>
<box>
<size>0.042 0.042 0.74</size>
</box>
</geometry>
</collision>
<collision name="base">
<pose>0 0 0.02 0 0 0</pose>
<geometry>
<box>
<size>0.56 0.56 0.04</size>
</box>
</geometry>
<surface>
<friction>
<torsional>
<coefficient>1.0</coefficient>
<use_patch_radius>false</use_patch_radius>
<surface_radius>0.05</surface_radius>
</torsional>
</friction>
</surface>
</collision>
<visual name="visual">
<geometry>
<mesh>
<uri>model://sorting_demo/models/table/meshes/cafe_table.dae</uri>
</mesh>
</geometry>
</visual>
</link>
</model>
</sdf>

72
src/sorting_demo/models/tray/tray.urdf.xacro Normal file
Просмотреть файл

@ -0,0 +1,72 @@
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="tray">
<xacro:property name="tray_width" value="0.2" />
<xacro:property name="tray_length" value="0.32" />
<xacro:property name="tray_height" value="0.05" />
<xacro:property name="tray_thickness" value="0.015" />
<xacro:property name="tray_density" value="100" />
<xacro:property name="tray_base_name" value="tray_base" />
<xacro:macro name="tray_part" params="name width length height mass material xPos:=0 yPos:=0">
<link name="${name}">
<inertial>
<mass value="${tray_density * width * length * height}" />
<inertia ixx="${(1/12) * tray_density * width * length * height * (length**2 + height**2)}" ixy="0.0" ixz="0.0"
iyy="${(1/12) * tray_density * width * length * height * (width**2 + height**2)}" iyz="0.0"
izz="${(1/12) * tray_density * width * length * height * (width**2 + length**2)}" />
</inertial>
<visual>
<geometry>
<box size="${width} ${length} ${height}" />
</geometry>
</visual>
<collision>
<geometry>
<box size="${width} ${length} ${height}" />
</geometry>
</collision>
</link>
<gazebo reference="${name}">
<material>${material}</material>
<mu1>1000</mu1>
<mu2>1000</mu2>
<collision>
<surface>
<friction>
<torsional>
<coefficient>0.456</coefficient>
<use_patch_radius>true</use_patch_radius>
<patch_radius>0.05</patch_radius>
</torsional>
</friction>
</surface>
</collision>
</gazebo>
<gazebo>
<static>true</static>
</gazebo>
<xacro:if value="${xPos != 0 or yPos != 0}">
<joint name="joint_${name}" type="fixed">
<origin xyz="${xPos * (tray_width - tray_thickness) / 2} ${yPos * (tray_length - tray_thickness) / 2} ${(tray_height + tray_thickness) / 2}" />
<parent link="${tray_base_name}" />
<child link="${name}" />
</joint>
</xacro:if>
</xacro:macro>
<xacro:tray_part name="${tray_base_name}" width="${tray_width}" length="${tray_length}" height="${tray_thickness}" mass="0.01" material="Gazebo/White" />
<xacro:tray_part name="tray_wall0" width="${tray_width - 2 * tray_thickness}" length="${tray_thickness}" height="${tray_height}" mass="0.01" material="Gazebo/White" yPos="-1" />
<xacro:tray_part name="tray_wall1" width="${tray_width - 2 * tray_thickness}" length="${tray_thickness}" height="${tray_height}" mass="0.01" material="Gazebo/White" yPos="1" />
<xacro:tray_part name="tray_wall2" width="${tray_thickness}" length="${tray_length}" height="${tray_height}" mass="0.01" material="Gazebo/White" xPos="-1" />
<xacro:tray_part name="tray_wall3" width="${tray_thickness}" length="${tray_length}" height="${tray_height}" mass="0.01" material="Gazebo/White" xPos="1" />
</robot>

97
src/sorting_demo/package.xml Normal file
Просмотреть файл

@ -0,0 +1,97 @@
<?xml version="1.0"?>
<package format="2">
<name>sorting_demo</name>
<version>0.0.0</version>
<description>The sorting demo package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="pblasco@plainconcepts.com">Pablo Iñigo Blasco</maintainer>
<maintainer email="mcsanchez@plainconcepts.com">Manuel Caballero Sánchez</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>Microsoft Copyright</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/sorting_demo</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>rospy</build_depend>
<build_depend>intera_core_msgs</build_depend>
<build_depend>gazebo_msgs</build_depend>
<build_depend>joint_state_controller</build_depend>
<build_depend>position_controllers</build_depend>
<build_depend>velocity_controllers</build_depend>
<build_depend>force_torque_sensor_controller</build_depend>
<build_depend>imu_sensor_controller</build_depend>
<build_depend>ros_control</build_depend>
<build_depend>joint_trajectory_controller</build_depend>
<build_depend>gripper_action_controller</build_depend>
<build_depend>gazebo_ros_control</build_depend>
<build_depend>gazebo_plugins</build_depend>
<build_depend>moveit</build_depend>
<build_depend>rospy-message-converter</build_depend>
<build_depend>gazebo_plugins</build_depend>
<build_depend>python-flask</build_depend>
<build_depend>python-catkin-tools</build_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>rospack</exec_depend>
<exec_depend>intera_core_msgs</exec_depend>
<exec_depend>intera_interface</exec_depend>
<exec_depend>sawyer_gazebo</exec_depend>
<exec_depend>gazebo_ros</exec_depend>
<exec_depend>gazebo_msgs</exec_depend>
<exec_depend>joint_state_controller</exec_depend>
<exec_depend>moveit</exec_depend>
<exec_depend>position_controllers</exec_depend>
<exec_depend>velocity_controllers</exec_depend>
<exec_depend>force_torque_sensor_controller</exec_depend>
<exec_depend>imu_sensor_controller</exec_depend>
<exec_depend>ros_control</exec_depend>
<exec_depend>joint_trajectory_controller</exec_depend>
<exec_depend>gripper_action_controller</exec_depend>
<exec_depend>rospy-message-converter</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
<gazebo_ros gazebo_model_path="${prefix}/.." />
</export>
</package>

23
src/sorting_demo/scripts/test_moveit.py Executable file
Просмотреть файл

@ -0,0 +1,23 @@
#! /usr/bin/env python
import geometry_msgs
import rospy
from sorting_demo.trajectory_planner import TrajectoryPlanner
if __name__ == "__main__":
rospy.init_node('move_group_python_interface_tutorial',
anonymous=True)
planner = TrajectoryPlanner()
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.7
pose_target.position.y = 0.0
pose_target.position.z = 0.5
#planner.move_to_cartesian_target(pose_target)
planner.move_to_joint_target([0.0,0.0,0.0,0.0,0.0,0.0,0.0])

29
src/sorting_demo/setup.py Executable file
Просмотреть файл

@ -0,0 +1,29 @@
#!/usr/bin/env python
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
"""
setup_args = generate_distutils_setup(
package=['sorting_demo'],
package_dir={'': 'src'},
)
setup(**setup_args)
"""
d = generate_distutils_setup()
d['packages'] = ['sorting_demo',
'sorting_demo.utils',
'sorting_demo.object_detection',
'intera_interface', 'intera_control',
'intera_dataflow',
'intera_io',
'intera_joint_trajectory_action',
'intera_motion_interface']
d['package_dir'] = {'': 'src', 'utils': 'src/utils'}
d['scripts']=['scripts/test_moveit.py']
setup(**d)

Двоичные данные
src/sorting_demo/share/head_mask.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 13 KiB

Двоичные данные
src/sorting_demo/share/test_head_simulator/1_color.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 153 KiB

Двоичные данные
src/sorting_demo/share/test_head_simulator/2_colors.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 153 KiB

Двоичные данные
src/sorting_demo/share/test_head_simulator/base_1.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 151 KiB

Двоичные данные
src/sorting_demo/share/test_head_simulator/base_2.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 151 KiB

Двоичные данные
src/sorting_demo/share/test_head_simulator/grouping.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 155 KiB

Двоичные данные
src/sorting_demo/share/test_head_simulator/joined_blobs.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 153 KiB

Двоичные данные
src/sorting_demo/share/test_head_simulator/no_cubes.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 152 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/angles1.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 114 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/angles2.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 114 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/border.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 110 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/debug0.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 114 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/debug1.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 100 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/debug2.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 106 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/debug3.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 107 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/debug4.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 98 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/debug5.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 93 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/debug6.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 88 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/debug7.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 84 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/debug8.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 87 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/joined_cubes_1.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 86 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/joined_cubes_2.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 87 KiB

Двоичные данные
src/sorting_demo/share/test_right_hand_simulator/many.png Normal file
Просмотреть файл

Двоичный файл не отображается.
Режим "рядом"

После

Ширина:  |  Высота:  |  Размер: 108 KiB

19
src/sorting_demo/share/testobstacle.txt Normal file
Просмотреть файл

@ -0,0 +1,19 @@
box_scene
* box1
1
box
0.1 0.2 0.3
1.0 1.0 1.0
0 0 0 1
0 1 0 1
.
#box_scene # maybe scene name
#* box1 # object name
#1 # number of shapes
#box # type of shape (box, cylinder, sphere, I think)
#0.1 0.2 0.3 # size of shape
#1.0 1.0 1.0 # position of shape
#0 0 0 1 # orientation of shape
#0 1 0 1 # RGBA color of shape
#. # necessary final dot?

206
src/sorting_demo/share/web_view.html Normal file
Просмотреть файл

@ -0,0 +1,206 @@
<html>
<head>
<style>
button {
color: blue;
width: 250px;
padding: 5px;
margin: 5px;
display: block
}
body{
background: #f0f0f0;
}
.column {
float: left;
width: 20%;
}
/* Clear floats after the columns */
.row:after {
content: "";
display: table;
clear: both;
}
</style>
<script>
function update(resturl, p1 = "", p2 = "") {
xmlhttp = new XMLHttpRequest();
xmlhttp.onreadystatechange = function() {
statediv = document.getElementById('statediv')
statediv.innerHTML = "hello"
if (this.readyState == 4 && this.status == 200) {
ugly = this.responseText;
var obj = JSON.parse(ugly);
updatePlanner(obj.result)
updateGrip(obj.result)
updateTable(obj.result)
updateTrays(obj.result.trays)
// Restore json result
obj = JSON.parse(ugly);
var pretty = JSON.stringify(obj, undefined, 4);
statediv.innerHTML = pretty;
} else if (this.status == 0) {
statediv.innerHTML = "Waiting server response..."
} else {
statediv.innerHTML = "status:" + this.status + "\n" + JSON.stringify(this.responseText)
}
};
finalurl = "http://localhost:5000/" + resturl
if (p1 != "") {
e = document.getElementById(p1);
param = e.options[e.selectedIndex].value;
finalurl = finalurl + "/" + param
}
if (p2 != "") {
e = document.getElementById(p2);
param = e.options[e.selectedIndex].value;
finalurl = finalurl + "/" + param
}
document.getElementById('note').innerHTML = finalurl
xmlhttp.open("GET", finalurl, true);
xmlhttp.setRequestHeader("Content-type", "application/x-www-form-urlencoded");
xmlhttp.send();
}
function updateTable(result) {
blocks = result.table_state.blocks;
for (i in blocks) {
var block = blocks[i];
// Clean response
delete block["table_pose"];
}
document.getElementById('tablestate').innerHTML = JSON.stringify(blocks, undefined, 4);
}
function updateGrip(result) {
gripstate = result.gripper.holding_block;
text = "Grip has no block"
if (gripstate != null) {
delete gripstate["table_pose"]
text = JSON.stringify(gripstate, undefined, 4);
}
document.getElementById('gripstate').innerHTML = text;
}
function updatePlanner(result) {
plannerstate = result.planner_state;
task = JSON.stringify(result.current_task, undefined, 4);
document.getElementById('plannerstate').innerHTML = plannerstate + " : " + task;
}
function updateTrays(trays) {
blocks = trays.blocks;
updateTray("tray1", trays[0]);
updateTray("tray2", trays[1]);
updateTray("tray3", trays[2]);
}
function updateTray(key, tray) {
text = "no block";
if (tray != null) {
delete tray["pose"];
for (i in tray.blocks) {
var block = tray.blocks[i];
// Clean response
delete block["table_pose"];
}
text = JSON.stringify(tray.blocks, undefined, 4);
}
document.getElementById(key).innerHTML = text;
}
</script>
</head>
<body onload="update('state')">
<div class="row">
<div class="column">
<h1>REST Endpoints</h1>
<h3>Observers</h3>
<button type="button" onclick="update('state')">/state</button>
<button type="button" onclick="update('current_task')">/current_task</button>
<button type="button" onclick="update('count_table_pieces','color')">/count_table_pieces/[color]</button>
<button type="button" onclick="update('current_piece_color')">/current_piece_color</button>
<h3>Lifecycle</h3>
<button type="button" onclick="update('pause')">/pause</button>
<button type="button" onclick="update('resume')">/resume</button>
<button type="button" onclick="update('start')">/start</button>
<button type="button" onclick="update('stop')">/stop</button>
<h3>Operations Primitives</h3>
<button type="button" onclick="update('pick_block_by_color', 'color')">/pick_block_by_color/[color]</button>
<button type="button" onclick="update('place_block_to_tray', 'tray')">/place_block_to_tray/[trayid]</button>
<h3>Others</h3>
<button type="button" onclick="update('greet')">/greet</button>
<button type="button" onclick="update('put_all_contents_on_table')">/put_all_contents_on_table</button>
<button type="button" onclick="update('put_block_into_tray', 'color','tray')">/put_block_into_tray/[color]/[trayid]</button>
</div>
<div class="column">
<h2>Params</h2>
<div>
p1:
<select id=color>
<option>Red</option>
<option>Green</option>
<option>Blue</option>
</select>
</div>
<div>
p2:
<select id=tray>
<option value="0">Tray 1</option>
<option value="1">Tray 2</option>
<option value="2">Tray 3</option>
</select>
</div>
<div>
<h2>Planer state: </h2>
<div id="plannerstate"></div>
</div>
<div>
<h2>Table:</h2>
<textarea id="tablestate" rows="40" cols="40"></textarea>
</div>
</div>
<div class="column">
<div>
<h2>Tray 1: </h2>
<textarea id="tray1" cols="40" rows="10"></textarea>
</div>
<div>
<h2>Tray 2:</h2>
<textarea id="tray2" cols="40" rows="10"></textarea>
</div>
<div>
<h2>Tray 3:</h2>
<textarea id="tray3" cols="40" rows="10"></textarea>
</div>
<div>
<h2>Grip:</h2>
<textarea id="gripstate" cols="40" rows="6"></textarea>
</div>
</div>
<div class="column">
<div id="note"></div>
<div>
<textarea id="statediv" cols=100 rows=60></textarea>
</div>
</div>
</div>
</body>
</html>

0
src/sorting_demo/src/__init__.py Normal file
Просмотреть файл

0
src/sorting_demo/src/sorting_demo/__init__.py Executable file
Просмотреть файл

0
src/sorting_demo/src/sorting_demo/concepts/__init__.py Normal file
Просмотреть файл

63
src/sorting_demo/src/sorting_demo/concepts/block.py Normal file
Просмотреть файл

@ -0,0 +1,63 @@
import re
from rospy_message_converter import message_converter
import demo_constants
class BlockState:
regex = re.compile(r'block(\d+)\.*')
def __init__(self, id, pose):
self.perception_id = None
self.gazebo_id = id
self.pose = pose
self.color = None
self.homogeneous_transform = None
if not demo_constants.is_real_robot():
search = BlockState.regex.search(id)
self.num = int(search.group(1))
else:
self.num = int(id)
self.gazebo_pose = None
self.grasp_pose = None
self.place_pose = None
self.table_grasp_pose = None
self.tray_place_pose = None
self.table_place_pose = None
# the 2d blob point estimabion based on the head_image processing
self.hue_estimation = None
# the color estimation based on the head image processing
self.hue_estimation = None
# the 3d pose estimation from the head image processing
self.headview_pose_estimation = None
self.tabletop_arm_view_estimated_pose = None
self.traytop_arm_view_estimated_pose =None
self.tray = None
def __str__(self):
return "[Block estpos = %s]" % str(self.headview_pose_estimation)
def get_state(self):
return {"id": self.perception_id , "table_pose": message_converter.convert_ros_message_to_dictionary(self.gazebo_pose),
"color": self.color}
@staticmethod
def is_block(id):
num = BlockState.regex.search(id)
return num is not None
def get_color(self):
return self.color
def is_color(self, color):
return color.upper() in self.color.upper()

5
src/sorting_demo/src/sorting_demo/concepts/gripper_state.py Normal file
Просмотреть файл

@ -0,0 +1,5 @@
class GripperState:
def __init__(self):
self.holding_block = None
def get_state(self):
return {"holding_block": None if self.holding_block is None else self.holding_block.get_state()}

17
src/sorting_demo/src/sorting_demo/concepts/table.py Normal file
Просмотреть файл

@ -0,0 +1,17 @@
class Table:
def __init__(self):
self.blocks = []
def get_blocks(self):
return self.blocks
def get_state(self):
return {"blocks": [b.get_state() for b in self.blocks]}
def notify_gripper_pick(self, b,gripper):
self.blocks.remove(b)
gripper.holding_block = b
def notify_gripper_place(self, b, gripper):
gripper.holding_block = None
self.blocks.append(b)

82
src/sorting_demo/src/sorting_demo/concepts/tray.py Normal file
Просмотреть файл

@ -0,0 +1,82 @@
import copy
import re
import math
import rospy
from rospy_message_converter import message_converter
import tf
import tf.transformations
from geometry_msgs.msg import Quaternion
class TrayState:
regex = re.compile(r'tray(\d+)\.*')
def __init__(self, gazebo_id, pose, TRAY_SURFACE_THICKNESS=0.04):
self.gazebo_id = gazebo_id
self.pose = pose
self.gazebo_pose = None
self.TRAY_SURFACE_THICKNESS =TRAY_SURFACE_THICKNESS
search = TrayState.regex.search(gazebo_id)
self.num = int(search.group(1))
self.blocks = []
@staticmethod
def is_tray(id):
num = TrayState.regex.search(id)
return num is not None
def notify_place_block(self, block, gripper_state):
block.tray = self
self.blocks.append(block)
gripper_state.holding_block = None
def notify_pick_block(self, block, gripper_state):
block.tray = None
self.blocks.remove(block)
gripper_state.holding_block = block
def get_tray_pick_location(self):
"""
provides the grasping pose for the tray
:return: geometry_msg/Pose
"""
copyfinalpose = copy.deepcopy(self.gazebo_pose)
copyfinalpose.position.y -= 0.15
copyfinalpose.position.z -= 0.02
return copyfinalpose
def get_tray_place_block_pose(self):
#return self.gazebo_pose
copygazebopose = copy.deepcopy(self.gazebo_pose)
yoffset = -0.08 + 0.075 * len(self.blocks)
copygazebopose.position.y -= yoffset
copygazebopose.position.z += self.TRAY_SURFACE_THICKNESS
zrot = tf.transformations.quaternion_from_euler(0, 0, -math.pi/2.0)
trayorientatin = [copygazebopose.orientation.x, copygazebopose.orientation.y, copygazebopose.orientation.z, copygazebopose.orientation.w]
# oorient = [overhead_orientation.x,overhead_orientation.y,overhead_orientation.z,overhead_orientation.w]
# resultingorient = tf.transformations.quaternion_multiply(cubeorientation, tf.transformations.quaternion_conjugate(oorient))
resultingorient = tf.transformations.quaternion_multiply(trayorientatin, zrot)
# resultingorient = cubeorientation
copygazebopose.orientation = Quaternion(x=resultingorient[0], y=resultingorient[1], z=resultingorient[2],
w=resultingorient[3])
rospy.logwarn("Tray Place location (objects %d, %lf): " % (len(self.blocks), yoffset) + str(copygazebopose))
return copygazebopose
def get_state(self):
return {"pose": message_converter.convert_ros_message_to_dictionary(self.gazebo_pose), "blocks": [b.get_state() for b in self.blocks]}
def __str__(self):
return "Tray: "+ str(self.gazebo_id) + ",num: " + str(self.num) + " -> " + str(len(self.blocks)) + " items"

100
src/sorting_demo/src/sorting_demo/cv_detection_camera_helper.py Normal file
Просмотреть файл

@ -0,0 +1,100 @@
#!/usr/bin/python
import Queue
import numpy
import rospy
import tf
from sensor_msgs.msg import CameraInfo
from image_geometry import PinholeCameraModel
from visualization_msgs.msg import *
import intera_interface
class CameraHelper:
"""
A helper class to take pictures with the Sawyer camera and unproject points from the images
"""
def __init__(self, camera_name, base_frame, table_height):
"""
Initialize the instance
:param camera_name: The camera name. One of (head_camera, right_hand_camera)
:param base_frame: The frame for the robot base
:param table_height: The table height with respect to base_frame
"""
self.camera_name = camera_name
self.base_frame = base_frame
self.table_height = table_height
self.image_queue = Queue.Queue()
self.pinhole_camera_model = PinholeCameraModel()
self.tf_listener = tf.TransformListener()
camera_info_topic = "/io/internal_camera/{}/camera_info".format(camera_name)
camera_info = rospy.wait_for_message(camera_info_topic, CameraInfo)
self.pinhole_camera_model.fromCameraInfo(camera_info)
cameras = intera_interface.Cameras()
cameras.set_callback(camera_name, self.__show_image_callback, rectify_image=True)
def __show_image_callback(self, img_data):
"""
Image callback for the camera
:param img_data: The image received from the camera
"""
if self.image_queue.empty():
self.image_queue.put(img_data)
def take_single_picture(self):
"""
Take one picture from the specified camera
:returns: The image
"""
with self.image_queue.mutex:
self.image_queue.queue.clear()
cameras = intera_interface.Cameras()
cameras.start_streaming(self.camera_name)
image_data = self.image_queue.get(block=True, timeout=None)
cameras.stop_streaming(self.camera_name)
return image_data
def project_point_on_table(self, point):
"""
Projects the 2D point from the camera image on the table
:param point: The 2D point in the form (x, y)
:return: The 3D point in the coordinate space of the frame that was specified when the object was initialized
"""
# Get camera frame name
camera_frame = self.pinhole_camera_model.tfFrame()
# Check that both frames exist
if self.tf_listener.frameExists(self.base_frame) and self.tf_listener.frameExists(camera_frame):
# Get transformation
time = self.tf_listener.getLatestCommonTime(self.base_frame, camera_frame)
camera_translation_base, camera_orientation_base = self.tf_listener.lookupTransform(self.base_frame, camera_frame, time)
# Unproject point
unprojected_ray_camera = self.pinhole_camera_model.projectPixelTo3dRay(point)
# Rotate ray based on the frame transformation
camera_frame_rotation_matrix = tf.transformations.quaternion_matrix(camera_orientation_base)
unprojected_ray_base = numpy.dot(camera_frame_rotation_matrix[:3,:3], unprojected_ray_camera)
# Intersect ray with base plane
point_height = camera_translation_base[2] - self.table_height
factor = -point_height / unprojected_ray_base[2]
intersection = camera_translation_base + unprojected_ray_base * factor
return intersection

277
src/sorting_demo/src/sorting_demo/cv_detection_head.py Executable file
Просмотреть файл

@ -0,0 +1,277 @@
#!/usr/bin/python
import sys
import numpy
import os
from matplotlib import pyplot
import rospy
import cv2
import rospkg
from cv_bridge import CvBridge, CvBridgeError
from visualization_msgs.msg import *
from cv_detection_camera_helper import CameraHelper
def get_blobs_info(cv_image):
"""
Gets information about the colored blobs in the image
:param cv_image: The OpenCV image to get blobs from
:return: A dictionary containing an array of points for each colored blob found, represented by its hue.
For example, in an image with 2 red blobs and 3 blue blobs, it will return
{0: [(639, 558), (702, 555)], 120: [(567, 550), (698, 515), (648, 515)]}
"""
# Show original image
#cv2.imshow("Original image", cv_image)
# Apply blur
BLUR_SIZE = 3
cv_image_blur = cv2.GaussianBlur(cv_image, (BLUR_SIZE, BLUR_SIZE), 0)
#cv2.imshow("Blur", cv_image_blur)
# Apply ROI mask
mask_path = rospkg.RosPack().get_path('sorting_demo') + "/share/head_mask.png"
cv_mask = cv2.imread(mask_path)
cv_mask = cv2.cvtColor(cv_mask, cv2.COLOR_BGR2GRAY)
cv_image_masked = cv2.bitwise_and(cv_image_blur, cv_image_blur, mask=cv_mask)
#cv2.imshow("Masked original", cv_image_masked)
# HSV split
cv_image_hsv = cv2.cvtColor(cv_image_masked, cv2.COLOR_BGR2HSV)
cv_image_h, cv_image_s, cv_image_v = cv2.split(cv_image_hsv)
#cv2.imshow("Image H", cv_image_h)
#cv2.imshow("Image S", cv_image_s)
#cv2.imshow("Image V", cv_image_v)
# Apply CLAHE to Value channel
CLAHE_SIZE = 16
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(CLAHE_SIZE, CLAHE_SIZE))
cv_image_v_clahe = clahe.apply(cv_image_v)
# Merge channels
cv_image_clahe = cv2.merge((cv_image_h, cv_image_s, cv_image_v_clahe))
#cv_image_clahe_rgb = cv2.cvtColor(cv_image_clahe, cv2.COLOR_HSV2BGR)
#cv2.imshow("CLAHE", cv_image_clahe_rgb)
# Multiply Saturation and Value channels to separate the cubes, removing the table
cv_image_sv_multiplied = cv2.multiply(cv_image_s, cv_image_v_clahe, scale=1/255.0)
#cv2.imshow("Image S*V", cv_image_sv_multiplied)
# Binarize the result
BIN_THRESHOLD = 64
#_, cv_image_binary = cv2.threshold(cv_image_sv_multiplied, 0, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU)
_, cv_image_binary = cv2.threshold(cv_image_sv_multiplied, BIN_THRESHOLD, 255, cv2.THRESH_BINARY)
#cv2.imshow("Threshold", cv_image_binary)
# Erode the image to use as mask
EROSION_SIZE = 3
erosion_kernel = numpy.ones((EROSION_SIZE, EROSION_SIZE), numpy.uint8)
cv_image_binary_eroded = cv2.erode(cv_image_binary, erosion_kernel)
# Calculate H-channel histogram, applying the binarized image as mask
cv_image_h_histogram = cv2.calcHist([cv_image_h], [0], cv_image_binary_eroded, [180], [0, 179])
# Smoothen the histogram to find local maxima
HISTOGRAM_BLUR_SIZE = 11
histogram_length = len(cv_image_h_histogram)
cv_image_h_histogram_wrap = cv2.repeat(cv_image_h_histogram, 3, 1)
cv_image_h_histogram_smooth = cv2.GaussianBlur(cv_image_h_histogram_wrap, (HISTOGRAM_BLUR_SIZE, HISTOGRAM_BLUR_SIZE), 0)
cv_image_h_histogram_cut = cv_image_h_histogram_smooth[histogram_length : 2 * histogram_length]
# Collect peaks
histogram_peaks = [i for i in range(len(cv_image_h_histogram_cut))
if cv_image_h_histogram_cut[(i - 1) % histogram_length] < cv_image_h_histogram_cut[i] > cv_image_h_histogram_cut[(i + 1) % histogram_length]]
# Filter peaks
PEAK_MINIMUM = 100 # Ideally below the value of a single cube
PEAK_MAXIMUM = 500 # Ideally above the value of all the cubes of a single color
histogram_high_peaks = filter(lambda p : PEAK_MINIMUM < cv_image_h_histogram_cut[p] < PEAK_MAXIMUM, histogram_peaks)
#print(histogram_high_peaks)
#pyplot.clf()
#pyplot.plot(cv_image_h_histogram_cut)
#pyplot.pause(0.001)
# Process every color found in the histogram
blob_info = {}
cv_image_contours_debug = cv2.cvtColor(cv_image_binary_eroded, cv2.COLOR_GRAY2BGR)
for current_hue in histogram_high_peaks:
# Perform a Hue rotation that will be used to make detecting the edge colors easier (red in HSV corresponds to both 0 and 180)
HUE_AMPLITUDE = 5
cv_image_h_rotated = cv_image_h.astype(numpy.int16)
cv_image_h_rotated[:] -= current_hue
cv_image_h_rotated[:] += HUE_AMPLITUDE
cv_image_h_rotated = numpy.remainder(cv_image_h_rotated, 180)
cv_image_h_rotated = cv_image_h_rotated.astype(numpy.uint8)
#cv2.imshow("Hue rotation {}".format(histogram_high_peaks.index(current_hue)), cv_image_h_rotated)
# Binarize using range function
cv_image_h_inrange = cv2.inRange(cv_image_h_rotated, 0, HUE_AMPLITUDE * 2)
# Apply binary mask (consider that both black and the edge color have hue 0)
cv_image_h_masked = cv2.bitwise_and(cv_image_h_inrange, cv_image_h_inrange, mask=cv_image_binary_eroded)
#cv2.imshow("inRange {}".format(histogram_high_peaks.index(current_hue)), cv_image_h_masked)
# Find contours
_, contours, _ = cv2.findContours(cv_image_h_masked.copy(), cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
# Filter by area
MINIMUM_AREA_SIZE = 50
contours_filtered_area = filter(lambda cnt: cv2.contourArea(cnt) > MINIMUM_AREA_SIZE, contours)
# Calculate convex hull
convex_contours = [cv2.convexHull(cnt) for cnt in contours_filtered_area]
contour_color_hsv = numpy.array([[[current_hue, 255, 255]]], numpy.uint8)
contour_color_rgb = cv2.cvtColor(contour_color_hsv, cv2.COLOR_HSV2BGR)[0][0].tolist()
#cv2.drawContours(cv_image_contours_debug, convex_contours, -1, contour_color_rgb, 1)
# Find centroids
contour_moments = [cv2.moments(cnt) for cnt in convex_contours]
contour_centroids = [(int(moments["m10"] / moments["m00"]), int(moments["m01"] / moments["m00"])) for moments in contour_moments if moments["m00"] != 0]
for (cx, cy) in contour_centroids:
cv2.circle(cv_image_contours_debug, (cx, cy), 3, contour_color_rgb, cv2.FILLED)
# Collect data
blob_info[current_hue] = contour_centroids
cv2.imwrite("/tmp/head_contours.jpg", cv_image_contours_debug)
return blob_info
def __publish_marker(publisher, index, point):
"""
Publishes a cube-shaped marker
:param index: The marker index
:param point: The 3D position of the marker
"""
marker = Marker()
marker.header.frame_id = "base"
marker.id = index
marker.type = Marker.CUBE
marker.scale.x = 0.04
marker.scale.y = 0.04
marker.scale.z = 0.04
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.pose.position.x = point[0]
marker.pose.position.y = point[1]
marker.pose.position.z = point[2]
publisher.publish(marker)
def test_head_ros():
"""
Test the blob detection and CameraHelper class using ROS
"""
rospy.init_node('cv_detection_head_camera')
camera_name = "head_camera"
TABLE_HEIGHT = demo_constants.TABLE_HEIGHT
camera_helper = CameraHelper(camera_name, "base", TABLE_HEIGHT)
bridge = CvBridge()
publisher = rospy.Publisher("cube_position_estimation", Marker, queue_size=10)
try:
while not rospy.is_shutdown():
# Take picture
img_data = camera_helper.take_single_picture()
# Convert to OpenCV format
cv_image = bridge.imgmsg_to_cv2(img_data, "bgr8")
# Save for debugging
#cv2.imwrite("/tmp/debug.png", cv_image)
# Get color blobs info
blob_info = get_blobs_info(cv_image)
# Project the points on 3D space
points = [y for x in blob_info.values() for y in x]
for index, point in enumerate(points):
projected = camera_helper.project_point_on_table(point)
__publish_marker(publisher, index, projected)
# Wait for a key press
cv2.waitKey(1)
rospy.sleep(0.1)
except CvBridgeError, err:
rospy.logerr(err)
# Exit
cv2.destroyAllWindows()
def test_head_cam():
"""
Test the blob detection using a USB camera
"""
# Create a video capture object
capture = cv2.VideoCapture(1)
capture.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
while True:
# Capture a frame
ret, cv_image = capture.read()
if ret:
# Save for debugging
#cv2.imwrite("/tmp/debug.png", cv_image)
# Get color blobs info
blob_info = get_blobs_info(cv_image)
print(blob_info)
# Check for a press on the Escape key
if cv2.waitKey(1) & 0xFF == 27:
break
# Exit
capture.release()
cv2.destroyAllWindows()
def test_head_debug():
"""
Test the blob detection using images on disk
"""
# Get files
path = rospkg.RosPack().get_path('sorting_demo') + "/share/test_head_simulator"
files = [f for f in os.listdir(path) if os.path.isfile(os.path.join(path, f))]
#print(files)
# Process files
for f in files:
# Get image path
image_path = os.path.join(path, f)
print(image_path)
# Read image
cv_image = cv2.imread(image_path)
# Get color blobs info
blob_info = get_blobs_info(cv_image)
print(blob_info)
# Wait for a key press
cv2.waitKey(0)
# Exit
cv2.destroyAllWindows()
if __name__ == '__main__':
sys.exit(test_head_debug())

357
src/sorting_demo/src/sorting_demo/cv_detection_right_hand.py Executable file
Просмотреть файл

@ -0,0 +1,357 @@
#!/usr/bin/python
import sys
import numpy
import os
import math
import functools
import rospy
import cv2
import rospkg
from cv_bridge import CvBridge, CvBridgeError
from cv_detection_camera_helper import CameraHelper
imgindex = 0
def __create_mask_image_from_template(reference_image, template, pos_x, pos_y):
'''
Resize a template image to match a reference image size, placing the template's center in the given position, to be used as a mask
:param reference_image: The reference image that the returned mask is intended to be applied to. The returned mask will have the same size as this image
:param template: The template to resize
:param pos_x: The x position where to place the center of the template in the final image
:param pos_y: The y position where to place the center of the template in the final image
:return: The resized, correctly positioned template
'''
# Get image and template sizes
reference_image_height, reference_image_width = reference_image.shape
template_height, template_width = template.shape
# Get the position the template should be placed at
pos_x_corrected = pos_x - template_width // 2
pos_y_corrected = pos_y - template_height // 2
# Calculate bottom and right margins
bottom_margin = reference_image_height - template_height - pos_y_corrected
right_margin = reference_image_width - template_width - pos_x_corrected
# Add the borders to the template image
border_top = max(0, pos_y_corrected)
border_bottom = max(0, bottom_margin)
border_left = max(0, pos_x_corrected)
border_right = max(0, right_margin)
mask_image = cv2.copyMakeBorder(template, border_top, border_bottom, border_left, border_right, cv2.BORDER_CONSTANT, value=0)
# Crop the image, in case the template ended up outside of the image
crop_top = int(math.fabs(min(0, pos_y_corrected)))
crop_bottom = crop_top + reference_image_height
crop_left = int(math.fabs(min(0, pos_x_corrected)))
crop_right = crop_left + reference_image_width
mask_image_cropped = mask_image[crop_top:crop_bottom,crop_left:crop_right]
return mask_image_cropped
def __rotate_image_size_corrected(image, angle):
# Calculate max size for the rotated template and image offset
image_size_height, image_size_width = image.shape
image_center_x = image_size_width // 2
image_center_y = image_size_height // 2
# Create rotation matrix
rotation_matrix = cv2.getRotationMatrix2D((image_center_x, image_center_y), -angle, 1)
# Apply offset
new_image_size = int(math.ceil(cv2.norm((image_size_height, image_size_width), normType=cv2.NORM_L2)))
rotation_matrix[0, 2] += (new_image_size - image_size_width) / 2
rotation_matrix[1, 2] += (new_image_size - image_size_height) / 2
# Apply rotation to the template
image_rotated = cv2.warpAffine(image, rotation_matrix, (new_image_size, new_image_size))
return image_rotated
def __apply_template_matching(angle, template, image):
# Rotate the template
template_rotated = __rotate_image_size_corrected(template, angle)
# Apply template matching
image_templated = cv2.matchTemplate(image, template_rotated, cv2.TM_CCOEFF_NORMED)
# Correct template matching image size difference
template_rotated_height, template_rotated_width = template_rotated.shape
template_half_height = template_rotated_height // 2
template_half_width = template_rotated_width // 2
image_templated_inrange_size_corrected = cv2.copyMakeBorder(image_templated, template_half_height, template_half_height, template_half_width, template_half_width, cv2.BORDER_CONSTANT, value=0)
# Calculate maximum match coefficient
max_match = numpy.max(image_templated_inrange_size_corrected)
return (max_match, angle, template_rotated, image_templated_inrange_size_corrected)
def get_cubes_z_rotation(cv_image, CUBE_SIZE=90):
"""
Gets the cubes rotation in the Z plane from an image. The results are sorted by distance to the center of the image
:param cv_image: The OpenCV image to get the cubes from
:return: An array containing the positions, angles and clearances of the cubes that have been found. The returned angles lie in the interval [-45, 45)
For example:
[((388, 526), -41.0, True, True), ((556, 524), -31.0, True, True), ((474, 382), -31.0, True, False)]
"""
# Show original image
#cv2.imshow("Original image", cv_image)
# Convert to grayscale
cv_image_grayscale = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
# Apply blur
BLUR_SIZE = 3
cv_image_blur = cv2.GaussianBlur(cv_image_grayscale, (BLUR_SIZE, BLUR_SIZE), 0)
#cv2.imshow("Blur", cv_image_blur)
# Apply CLAHE
CLAHE_SIZE = 64
clahe = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(CLAHE_SIZE, CLAHE_SIZE))
cv_image_clahe = clahe.apply(cv_image_blur)
#cv2.imshow("CLAHE", cv_image_clahe)
# Apply Canny filter
sigma = 0.33
median = numpy.median(cv_image_clahe)
lower = int(max(0, (1.0 - sigma) * median))
upper = int(min(255, (1.0 + sigma) * median))
cv_image_canny = cv2.Canny(cv_image_clahe, lower, upper)
#cv2.imshow("Canny", cv_image_canny)
# Apply dilation
DILATION_SIZE = 5
dilation_kernel = numpy.ones((DILATION_SIZE, DILATION_SIZE), numpy.uint8)
cv_image_dilated = cv2.dilate(cv_image_canny, dilation_kernel, iterations = 1)
#cv2.imshow("Dilation", cv_image_dilated)
# Find contours
_, contours, _ = cv2.findContours(cv_image_dilated.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
# Simplify contours
#contours_approx = [cv2.approxPolyDP(cnt, 0.005 * cv2.arcLength(cnt, True), True) for cnt in contours]
# Draw image with filled contours
cv_image_height, cv_image_width = cv_image_grayscale.shape
cv_image_contours_filled = numpy.zeros((cv_image_height, cv_image_width), numpy.uint8)
cv_image_contours_filled = cv2.fillPoly(cv_image_contours_filled, pts=contours, color=255)
#cv2.imshow("Contours", cv_image_contours_filled)
# Create cube image for template matching
cv_image_cube_template = numpy.full((CUBE_SIZE, CUBE_SIZE, 1), 255, numpy.uint8)
CUBE_BORDER_SIZE = 4
cv_image_cube_template_border = cv2.copyMakeBorder(cv_image_cube_template, CUBE_BORDER_SIZE, CUBE_BORDER_SIZE, CUBE_BORDER_SIZE, CUBE_BORDER_SIZE, cv2.BORDER_CONSTANT, value=0)
# Create mask for clearance check
CLEARANCE_AREA_LENGTH = CUBE_SIZE / 2
CLEARANCE_AREA_MARGIN = 20
clearance_check_mask = numpy.full((CUBE_SIZE + 2 * CLEARANCE_AREA_MARGIN, CUBE_SIZE), 0, numpy.uint8)
clearance_check_mask = cv2.copyMakeBorder(clearance_check_mask, CLEARANCE_AREA_LENGTH, CLEARANCE_AREA_LENGTH, 0, 0, cv2.BORDER_CONSTANT, value=255)
# Calculate the angles that will be used when rotating the template (between -45 and 45 because of square symmetry)
range_subdivisions = 90
all_angles = numpy.arange(-45, 45, 90.0 / range_subdivisions)
# Look for cubes in the image and erase them until none are found
cube_positions_and_angles = []
original_image_loop = cv_image_contours_filled.copy()
while True:
#cv2.imshow("Loop image", original_image_loop)
#cv2.waitKey(0)
# Apply template matching rotating the template
apply_template_matching_partial = functools.partial(__apply_template_matching, template=cv_image_cube_template_border, image=original_image_loop)
template_matching_results = map(apply_template_matching_partial, all_angles)
# Get max matching coefficient
template_matching_results_max_values = [value for value, _, _, _ in template_matching_results]
template_matching_results_max = max(template_matching_results_max_values)
# Check if the match coefficient is good enough
TEMPLATE_MATCHING_MAX_THRESHOLD = 0.5
if template_matching_results_max < TEMPLATE_MATCHING_MAX_THRESHOLD:
break
# Collect best match
template_matching_results_max_index = template_matching_results_max_values.index(template_matching_results_max)
_, angle, template_rotated, image_templated = template_matching_results[template_matching_results_max_index]
# Find location
_, _, _, (max_loc_x, max_loc_y) = cv2.minMaxLoc(image_templated)
# Apply template as a mask to the original image, deleting the area it matched
template_mask = __create_mask_image_from_template(original_image_loop, template_rotated, max_loc_x, max_loc_y)
template_mask_inverted = cv2.bitwise_not(template_mask)
original_image_loop = cv2.bitwise_and(original_image_loop, template_mask_inverted)
# Rotate the clearance check mask to create the two needed for the clearance test
clearance_check_mask_rotated_0 = __rotate_image_size_corrected(clearance_check_mask, angle)
clearance_check_mask_rotated_90 = __rotate_image_size_corrected(clearance_check_mask, angle + 90)
#cv2.imshow("Clearance check mask rotated 0", clearance_check_mask_rotated_0)
#cv2.imshow("Clearance check mask rotated 90", clearance_check_mask_rotated_90)
# Create mask image from the clearance check mask
clearance_check_mask_full_size_0 = __create_mask_image_from_template(cv_image_contours_filled, clearance_check_mask_rotated_0, max_loc_x, max_loc_y)
clearance_check_mask_full_size_90 = __create_mask_image_from_template(cv_image_contours_filled, clearance_check_mask_rotated_90, max_loc_x, max_loc_y)
#cv2.imshow("Clearance check mask 0 full", clearance_check_mask_full_size_0)
#cv2.imshow("Clearance check mask 90 full", clearance_check_mask_full_size_90)
# Apply clearance mask to the original filled-contours image
original_image = cv_image_contours_filled
original_image_clearance_mask_applied_0 = cv2.bitwise_and(original_image, clearance_check_mask_full_size_0)
original_image_clearance_mask_applied_90 = cv2.bitwise_and(original_image, clearance_check_mask_full_size_90)
#cv2.imshow("Clearance check mask 0 applied", original_image_clearance_mask_applied_0)
#cv2.imshow("Clearance check mask 90 applied", original_image_clearance_mask_applied_90)
# Check clearance
clearance_0_count = cv2.countNonZero(original_image_clearance_mask_applied_0)
clearance_90_count = cv2.countNonZero(original_image_clearance_mask_applied_90)
CLEARANCE_THRESHOLD = 50
clearance_0 = clearance_0_count < CLEARANCE_THRESHOLD
clearance_90 = clearance_90_count < CLEARANCE_THRESHOLD
# Store result
cube_positions_and_angles.append(((max_loc_x, max_loc_y), angle, clearance_0, clearance_90))
# Sort cube positions by distance to the center of the image
image_center_x = cv_image_height // 2
image_center_y = cv_image_width // 2
image_center = (image_center_x, image_center_y)
cube_positions_and_angles_sorted = sorted(cube_positions_and_angles, key=lambda (position, angle, clearance_0, clearance_90) : cv2.norm(position, image_center, normType=cv2.NORM_L2))
# Draw debug image
template_matching_debug_image = cv_image.copy()
for ((x, y), angle, clear_0, clear_90) in cube_positions_and_angles_sorted:
rotated_rectangle = cv2.boxPoints(((x, y), (CUBE_SIZE, CUBE_SIZE), angle))
rotated_rectangle = numpy.int0(rotated_rectangle)
cv2.drawContours(template_matching_debug_image, [rotated_rectangle], -1, (255, 0, 0), 2)
clearance_points_rectangle = cv2.boxPoints(((x, y), (CUBE_SIZE * 0.6, CUBE_SIZE * 0.6), angle + 45))
clearance_points_rectangle = numpy.int0(clearance_points_rectangle)
clearance_bools = [clear_90, clear_0]
for (i, (point_x, point_y)) in enumerate(clearance_points_rectangle):
current_clearance = clearance_bools[i % len(clearance_bools)]
clearance_circle_color = current_clearance and (0, 255, 0) or (0, 0, 255)
cv2.circle(template_matching_debug_image, (point_x, point_y), 5, clearance_circle_color, cv2.FILLED)
cv2.circle(template_matching_debug_image, (point_x, point_y), 5, (255, 255, 255), 2)
#cv2.imshow("Template matching result", template_matching_debug_image)
global imgindex
cv2.imwrite("/tmp/img"+ str(imgindex)+".jpg", template_matching_debug_image)
imgindex+=1
#cv2.waitKey(0)
return cube_positions_and_angles_sorted
def test_right_hand_ros():
"""
Test the cube orientation sensing using ROS
"""
rospy.init_node('cv_detection_right_hand_camera')
camera_name = "right_hand_camera"
camera_helper = CameraHelper(camera_name, "base", 0)
bridge = CvBridge()
try:
while not rospy.is_shutdown():
# Take picture
img_data = camera_helper.take_single_picture()
# Convert to OpenCV format
cv_image = bridge.imgmsg_to_cv2(img_data, "bgr8")
# Save for debugging
#cv2.imwrite("/tmp/debug.png", cv_image)
# Get cube rotation
angles = get_cubes_z_rotation(cv_image)
print(angles)
# Wait for a key press
cv2.waitKey(1)
rospy.sleep(0.1)
except CvBridgeError, err:
rospy.logerr(err)
# Exit
cv2.destroyAllWindows()
def test_right_hand_cam():
"""
Test the blob detection using a USB camera
"""
# Create a video capture object
capture = cv2.VideoCapture(1)
capture.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
while True:
# Capture a frame
ret, cv_image = capture.read()
if ret:
# Save for debugging
#cv2.imwrite("/tmp/debug.png", cv_image)
# Get cube rotation
angles = get_cubes_z_rotation(cv_image)
print(angles)
# Check for a press on the Escape key
if cv2.waitKey(1) & 0xFF == 27:
break
# Exit
capture.release()
cv2.destroyAllWindows()
def test_right_hand_debug():
"""
Test the cube orientation sensing using images on disk
"""
# Get files
path = rospkg.RosPack().get_path('sorting_demo') + "/share/test_right_hand_simulator"
files = [f for f in os.listdir(path) if os.path.isfile(os.path.join(path, f))]
#files = ["border.png"]
#print(files)
# Process files
for f in files:
# Get image path
image_path = os.path.join(path, f)
print(image_path)
# Read image
cv_image = cv2.imread(image_path)
# Get cube rotation
angles = get_cubes_z_rotation(cv_image)
print(angles)
# Wait for a key press
cv2.waitKey(0)
# Exit
cv2.destroyAllWindows()
if __name__ == '__main__':
sys.exit(test_right_hand_debug())

34
src/sorting_demo/src/sorting_demo/demo_constants.py Normal file
Просмотреть файл

@ -0,0 +1,34 @@
#!/usr/bin/python
import rospy
CUBE_EDGE_LENGTH = 0.04
BLOCK_COLOR_MAPPINGS = [
{"material": "Gazebo/Green"},
{"material": "Gazebo/Red"},
{"material": "Gazebo/Blue"},
{"material": "Gazebo/Green"},
{"material": "Gazebo/Red"},
{"material": "Gazebo/Blue"},
{"material": "Gazebo/Red"},
{"material": "Gazebo/Blue"},
{"material": "Gazebo/Green"}
]
TRAY_COLORS = ["Red", "Green", "Blue"]
TABLE_HEIGHT = -0.15
TRAY_SURFACE_THICKNESS = 0.04
ARM_TOP_VIEW_Z_OFFSET = 0.05 # meters
SIMULATE_TRAY_BLOCK_DETECTION = True
def is_real_robot():
if rospy.has_param("/use_sim_time"):
return not rospy.get_param("/use_sim_time")
else:
return False

469
src/sorting_demo/src/sorting_demo/environment_estimation.py Normal file
Просмотреть файл

@ -0,0 +1,469 @@
#!/usr/bin/python
import copy
import random
import re
import math
import rospy
import tf
import tf.transformations
from cv_bridge import CvBridge
from gazebo_msgs.msg import LinkStates, sys
from geometry_msgs.msg import Pose, Point, Quaternion
from concepts.block import BlockState
from concepts.tray import TrayState
from concepts.table import Table
from cv_detection_head import CameraHelper, get_blobs_info
from cv_detection_right_hand import get_cubes_z_rotation
from utils.mathutils import *
import demo_constants
from threading import RLock
import cv2
class EnvironmentEstimation:
def __init__(self):
"""
"""
self.gazebo_trays = []
self.gazebo_blocks = []
self.trays = []
self.blocks = []
self.tf_broacaster = tf.TransformBroadcaster()
self.tf_listener = tf.TransformListener()
# initial simulated implementation
pub = rospy.Subscriber('/gazebo/link_states', LinkStates, self.simulated_link_state_callback, queue_size=10)
self.gazebo_world_to_ros_transform = None
self.original_blocks_poses_ = None
self.mutex = RLock()
TABLE_HEIGHT = -0.12
self.head_camera_helper = CameraHelper("head_camera", "base", TABLE_HEIGHT)
self.bridge = CvBridge()
self.block_pose_estimation_head_camera = None
self.table = Table()
self.hand_camera_helper = CameraHelper("right_hand_camera", "base", TABLE_HEIGHT)
if demo_constants.is_real_robot():
k = 3
for i in xrange(k):
for j in xrange(k):
q = tf.transformations.quaternion_from_euler(random.uniform(0, 2 * math.pi),
random.uniform(0, 2 * math.pi),
random.uniform(0, 2 * math.pi))
block = BlockState(id=str(len(self.gazebo_blocks)),
pose=Pose(position=Point(x=0.45 + j * 0.15 + random.uniform(-1, 1) * 0.03,
y=-0.15 + i * 0.15 + random.uniform(-1, 1) * 0.03,
z=0.7725),
orientation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])))
self.gazebo_blocks.append(block)
def identify_block_from_aproximated_point(self, projected):
"""
:param projected:
:return:
"""
bestindex = -1
bestdist = sys.float_info.max
for index, b in enumerate(self.blocks):
p1 = b.pose.position
p2 = projected
dx = p1.x - p2[0]
dy = p1.y - p2[1]
dz = p1.z - p2[2]
dist = math.sqrt(dx * dx + dy * dy + dz * dz)
if dist < bestdist:
bestdist = dist
bestindex = index
if bestindex != -1:
return self.blocks[bestindex]
else:
return None
def compute_block_pose_estimation_from_arm_camera(self, CUBE_SIZE=150):
# get latest image from topic
rospy.sleep(0.3)
# Take picture
img_data = self.hand_camera_helper.take_single_picture()
# Convert to OpenCV format
cv_image = self.bridge.imgmsg_to_cv2(img_data, "bgr8")
camwtrans = None
camwrot = None
try:
(camwtrans, camwrot) = self.tf_listener.lookupTransform('/right_hand_camera_optical', '/base',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as ex:
rospy.logerr(ex.message)
rotmat = tf.transformations.quaternion_matrix(camwrot)
transmat = tf.transformations.translation_matrix((camwtrans))
zaxis = rotmat[2, :]
cameratransform = tf.transformations.concatenate_matrices(rotmat, transmat)
# empirically obtained with a drawing, and rviz... the optical z axis is the one that is looking fordware (world x)
# also weirdly the frame axis in the rotation matrix are rows instead of columns
camera_yaw_angle = math.atan2(zaxis[1], zaxis[0])
rospy.logwarn("camera angle:" + str(camera_yaw_angle * 180.0 / math.pi))
rospy.logwarn("camera rot:" + str(rotmat))
rospy.logwarn("zaxis camera vector:" + str(zaxis))
# Save for debugging
# cv2.imwrite("/tmp/debug.png", cv_image)
# Get cube rotation
detected_cubes_info = get_cubes_z_rotation(cv_image, CUBE_SIZE=CUBE_SIZE)
center = (cv_image.shape[1] / 2, cv_image.shape[0] / 2)
def cubedistToCenter(cube):
# ((370, 224), 26.0, True, False)
dx = cube[0][0] - center[0]
dy = cube[0][1] - center[1]
return dx * dx + dy * dy
sorted_center_cubes = sorted(detected_cubes_info, key=cubedistToCenter)
try:
cube = sorted_center_cubes[0]
image_cube_angle = cube[1] * (math.pi / 180.0)
graspA = cube[2]
graspB = cube[3]
rospy.logwarn("image detected cube angle: " + str(image_cube_angle))
final_cube_yaw_angle = camera_yaw_angle - image_cube_angle
while final_cube_yaw_angle > math.pi / 4:
final_cube_yaw_angle -= math.pi / 2
while final_cube_yaw_angle < -math.pi / 4:
final_cube_yaw_angle += math.pi / 2
# select the other grasping
if not graspA and graspB:
rospy.logwarn("Swiching grasping orientation for current block (grasping clearance)")
if final_cube_yaw_angle > 0:
final_cube_yaw_angle -= math.pi / 2
else:
final_cube_yaw_angle += math.pi / 2
projected = self.hand_camera_helper.project_point_on_table(cube[0])
poseq = tf.transformations.quaternion_from_euler(0, 0, final_cube_yaw_angle)
rospy.logwarn("quaternion angle:" + str(poseq))
self.tf_broacaster.sendTransform(projected, poseq, rospy.Time(0), "estimated_cube_1", "base")
rospy.logwarn(projected)
# cv2.imshow("cube detection", cv_image)
# cv2.waitKey(0)
return Pose(position=Point(x=projected[0], y=projected[1], z=projected[1]),
orientation=Quaternion(x=poseq[0], y=poseq[1], z=poseq[2], w=poseq[3])), graspA or graspB
except Exception as ex:
rospy.logwarn("erroneous cube detection")
# cv2.imshow("erroneus cube detection", cv_image)
# cv2.waitKey(0)
return None, False
def compute_block_pose_estimations_from_head_camera(self):
"""
For each block updates:
- block.headview_pose_estimation
- block.hue_estimation
:return:
"""
try:
self.mutex.acquire()
img_data = self.head_camera_helper.take_single_picture()
# Convert to OpenCV format
cv_image = self.bridge.imgmsg_to_cv2(img_data, "bgr8")
cv2.imwrite("/tmp/last_head_picture.jpg", cv_image)
rospy.logwarn("processing head camera image to find blocks")
blobs_info = get_blobs_info(cv_image)
index = 0
ptinfos = []
for huekey in blobs_info.keys():
points = blobs_info[huekey]
rospy.logwarn("blob position[%d]: %s" % (index, str(points)))
for point in points:
ptinfos.append([huekey, point])
index += 1
detected_blocks = []
for huekey, point2d in ptinfos:
projected = self.head_camera_helper.project_point_on_table(point2d)
rospy.logwarn("projected: %s" % str(projected))
block = self.identify_block_from_aproximated_point(projected)
if block is None:
continue
detected_blocks.append(block)
block.headview_proj_estimation = point2d
block.headview_proj_estimation = projected
block.hue_estimation = huekey
block.headview_pose_estimation = Pose(
position=Point(x=projected[0], y=projected[1], z=projected[2]),
orientation=Quaternion(x=0, y=0, z=0, w=1))
rospy.logwarn("blob identified: " + str(block))
rospy.logwarn("Table blocks:")
self.table.blocks = detected_blocks
for b in self.table.blocks:
rospy.logwarn(b)
self.blocks = self.table.blocks
for tray in self.trays:
rospy.logwarn("Tray blocks:")
for b in tray.blocks:
rospy.logwarn(b)
self.blocks = self.blocks + tray.blocks
rospy.logwarn("All known blocks:")
for b in self.blocks:
rospy.logwarn(b)
finally:
self.mutex.release()
def simulated_link_state_callback(self, links):
"""
string[] name
geometry_msgs/Pose[] pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
geometry_msgs/Twist[] twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
:param links:
:return:
"""
try:
self.mutex.acquire()
blocks = []
trays = []
base_index = [i for i, name in enumerate(links.name) if name == "sawyer::base"][0]
self.gazebo_world_to_ros_transform = links.pose[base_index]
for i, name in enumerate(links.name):
pose = links.pose[i]
if BlockState.is_block(name):
item = self.get_block_by_gazebo_id(name)
if item is None:
# rospy.logwarn("block create name: "+ name)
item = BlockState(id=name, pose=pose)
item.color = demo_constants.BLOCK_COLOR_MAPPINGS[item.num]["material"].replace("Gazebo/", "")
else:
item.pose = pose
blocks.append(item)
elif TrayState.is_tray(name):
item = self.get_tray_by_gazebo_id(name)
# item = None
if item is None:
item = TrayState(gazebo_id=name, pose=pose,
TRAY_SURFACE_THICKNESS=demo_constants.TRAY_SURFACE_THICKNESS)
item.color = demo_constants.TRAY_COLORS[item.num].replace("Gazebo/", "")
else:
item.pose = pose
trays.append(item)
else:
continue
# rospy.logwarn("simulated object state: " + name + " -> " + item.num)
self.gazebo_blocks = blocks
self.gazebo_trays = trays
except Exception as ex:
rospy.logerr(ex.message)
finally:
self.mutex.release()
def update(self):
"""
this method basically double buffers the state of block and trays. It also publishes tf.
For the simulated case it copies from gazebo_blocks and gazebo_trays to blocks and trays
:return:
"""
try:
self.mutex.acquire()
collections = [self.gazebo_blocks, self.gazebo_trays]
blocks = []
trays = []
if not demo_constants.is_real_robot():
# publish tfs
basehomopose = get_homo_matrix_from_pose_msg(self.gazebo_world_to_ros_transform, tag="base")
for items in collections:
for item in items:
# block homogeneous transform
homo_pose = get_homo_matrix_from_pose_msg(item.pose, tag="block")
# rospy.logwarn("TF PUBLISH..." + str(homo_pose))
# rospy.logwarn("item state: " + str(item))
transf_homopose = inverse_compose(basehomopose, homo_pose)
trans = tf.transformations.translation_from_matrix(transf_homopose)
quat = tf.transformations.quaternion_from_matrix(transf_homopose)
self.tf_broacaster.sendTransform(trans,
quat,
rospy.Time.now(),
item.gazebo_id,
"world")
item.gazebo_pose = homotransform_to_pose_msg(transf_homopose)
if isinstance(item, BlockState):
blocks.append(item)
elif isinstance(item, TrayState):
trays.append(item)
# else:
# rospy.logwarn("DETECTED ITEM:" + str(item))
self.blocks = blocks
self.trays = trays
# rospy.logwarn("GAZEBO blocks update lenght: %d"%len(self.gazebo_blocks))
# rospy.logwarn("blocks update lenght: %d"%len(self.blocks))
if self.original_blocks_poses_ is None:
self.original_blocks_poses_ = [copy.deepcopy(block.gazebo_pose) for block in blocks]
finally:
self.mutex.release()
def get_blocks(self):
"""
:return array of (name, geometry_msgs.msg.Pose)
"""
try:
self.mutex.acquire()
return [b for b in self.blocks]
finally:
self.mutex.release()
def get_block_by_gazebo_id(self, gazebo_id):
"""
:param gazebo_id:
:return:
"""
try:
self.mutex.acquire()
filtered_blocks = [block for block in self.blocks if block.gazebo_id == gazebo_id]
if len(filtered_blocks) == 0:
return None
else:
return filtered_blocks[0]
finally:
self.mutex.release()
def get_original_block_poses(self):
"""
:return:
"""
try:
self.mutex.acquire()
return [copy.deepcopy(p) for p in self.original_blocks_poses_]
finally:
self.mutex.release()
def get_tray_by_gazebo_id(self, gazebo_id):
"""
:param gazebo_id:
:return:
"""
try:
self.mutex.acquire()
filtered_trays = [tray for tray in self.trays if tray.gazebo_id == gazebo_id]
if len(filtered_trays) == 0:
return None
else:
return filtered_trays[0]
finally:
self.mutex.release()
def get_tray_by_color(self, color):
"""
:param id:
:return:
"""
color = color.replace("Gazebo/", "")
rospy.logwarn("by color: " + str(color))
rospy.logwarn("by color: " + str(self.trays))
filtered_trays = [tray for tray in self.trays if tray.color == color]
if len(filtered_trays) == 0:
return None
else:
return filtered_trays[0]
def get_tray_by_num(self, num):
"""
:param num:
:return:
"""
rospy.logwarn("by num: " + str(num))
rospy.logwarn("by trays: " + str([t.num for t in self.trays]))
filtered_trays = [tray for tray in self.trays if int(tray.num) == int(num)]
if len(filtered_trays) == 0:
return None
else:
return filtered_trays[0]
def get_trays(self):
"""
:return array of (name, geometry_msgs.msg.Pose)
"""
return self.trays

153
src/sorting_demo/src/sorting_demo/gazebo_models.py Normal file
Просмотреть файл

@ -0,0 +1,153 @@
#!/usr/bin/python
import math
import random
import sys
import rospkg
import rospy
import tf
import xacro
from geometry_msgs.msg import Pose, Point, Quaternion
from gazebo_msgs.srv import SpawnModel, DeleteModel
from demo_constants import BLOCK_COLOR_MAPPINGS, CUBE_EDGE_LENGTH
def spawn_urdf(name, description_xml, pose, reference_frame):
rospy.wait_for_service('/gazebo/spawn_urdf_model')
try:
spawn_urdf = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
resp_urdf = spawn_urdf(name, description_xml, "/", pose, reference_frame)
except rospy.ServiceException as e:
rospy.logerr("Spawn URDF service call failed: {0}".format(e))
def load_xacro_file(file_path, mappings):
urdf_doc = xacro.process_file(file_path, mappings=mappings)
urdf_xml = urdf_doc.toprettyxml(indent=' ', encoding='utf-8')
urdf_xml = urdf_xml.replace('\n', '')
return urdf_xml
def spawn_xacro_urdf_model(name, path, pose, reference_frame, mappings):
description_xml = load_xacro_file(path, mappings)
spawn_urdf(name, description_xml, pose, reference_frame)
def spawn_xacro_sdf_model(name, path, pose, reference_frame, mappings):
description_xml = load_xacro_file(path, mappings)
spawn_sdf(name, description_xml, pose, reference_frame)
def spawn_urdf_model(name, path, pose, reference_frame):
description_xml = ''
with open(path, "r") as model_file:
description_xml = model_file.read().replace('\n', '')
spawn_urdf(name, description_xml, pose, reference_frame)
def spawn_sdf(name, description_xml, pose, reference_frame):
rospy.wait_for_service('/gazebo/spawn_sdf_model')
try:
spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
resp_sdf = spawn_sdf(name, description_xml, "/", pose, reference_frame)
except rospy.ServiceException as e:
rospy.logerr("Spawn SDF service call failed: {0}".format(e))
def spawn_sdf_model(name, path, pose, reference_frame):
# Load Model SDF
description_xml = ''
with open(path, "r") as model_file:
description_xml = model_file.read().replace('\n', '')
spawn_sdf(name, description_xml, pose,reference_frame)
def load_gazebo_models():
model_list = []
world_reference_frame = "world"
# sorting_demo model path
sorting_demo_models_path = rospkg.RosPack().get_path('sorting_demo') + "/models/"
# Spawn Blocks Table
blocks_table_name = "blocks_table"
blocks_table_path = sorting_demo_models_path + "table/model.sdf"
blocks_table_pose = Pose(position=Point(x=0.75, y=0.0, z=0.0))
spawn_sdf_model(blocks_table_name, blocks_table_path, blocks_table_pose, world_reference_frame)
model_list.append(blocks_table_name)
# Spawn Trays Table
trays_table_name = "trays_table"
trays_table_path = sorting_demo_models_path + "table/model.sdf"
trays_table_pose = Pose(position=Point(x=0.0, y=0.95, z=0.0))
spawn_sdf_model(trays_table_name, trays_table_path, trays_table_pose, world_reference_frame)
model_list.append(trays_table_name)
# Spawn trays
tray_path = sorting_demo_models_path + "tray/tray.urdf.xacro"
tray_poses = [Pose(position=Point(x=-0.3, y=0.7, z=0.7828)),
Pose(position=Point(x=0.0, y=0.7, z=0.7828)),
Pose(position=Point(x=0.3, y=0.7, z=0.7828))]
for (i, pose) in enumerate(tray_poses):
name = "tray{}".format(i)
spawn_xacro_urdf_model(name, tray_path, pose, world_reference_frame, {})
model_list.append(name)
# Spawn blocks
block_path = sorting_demo_models_path + "block/block.urdf.xacro"
block_poses = []
k = 3
for i in xrange(k):
for j in xrange(k):
q = tf.transformations.quaternion_from_euler(random.uniform(0, 2 * math.pi), random.uniform(0, 2 * math.pi),
random.uniform(0, 2 * math.pi))
block_poses.append(Pose(position=Point(x=0.45 + j * 0.15 + random.uniform(-1, 1) * 0.03,
y=-0.15 + i * 0.15 + random.uniform(-1, 1) * 0.03, z=0.7725),
orientation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])))
#block_poses.append(Pose(position=Point(x= 0.45 + j*0.12 +random.uniform(-1, 1)*0.03 , y= -0.15 + i * 0.15 +random.uniform(-1, 1)*0.03, z=0.7725)))
"""
Pose(position=Point(x=0.60, y=0.1265, z=0.7725)),
Pose(position=Point(x=0.80, y=0.12, z=0.7725)),
Pose(position=Point(x=0.60, y=-0.1, z=0.7725)),
Pose(position=Point(x=0.80, y=-0.1, z=0.7725)),
Pose(position=Point(x=0.4225, y=-0.1, z=0.7725)),
Pose(position=Point(x=0.60, y=-0.35, z=0.7725)),
Pose(position=Point(x=0.80, y=-0.35, z=0.7725)),
Pose(position=Point(x=0.4225, y=-0.35, z=0.7725))
"""
for (i, (pose, color_mappings)) in enumerate(zip(block_poses, BLOCK_COLOR_MAPPINGS)):
name = "block{}".format(i)
mappings = {"edge_length" : str(CUBE_EDGE_LENGTH)}
mappings.update(color_mappings)
spawn_xacro_urdf_model(name, block_path, pose, world_reference_frame, mappings)
model_list.append(name)
return model_list, block_poses
def delete_gazebo_models(model_list):
# This will be called on ROS Exit, deleting Gazebo models
# Do not wait for the Gazebo Delete Model service, since
# Gazebo should already be running. If the service is not
# available since Gazebo has been killed, it is fine to error out
try:
delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
for model in model_list:
resp_delete = delete_model(model)
except rospy.ServiceException as e:
print("Delete Model service call failed: {0}".format(e))

281
src/sorting_demo/src/sorting_demo/goto_angles_in_contact.py Normal file
Просмотреть файл

@ -0,0 +1,281 @@
#! /usr/bin/env python
# Copyright (c) 2016-2018, Rethink Robotics Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from rospkg.distro import current_distro_codename
import rospy
import argparse
from geometry_msgs.msg import Pose
from intera_motion_interface import (
MotionTrajectory,
MotionWaypoint,
MotionWaypointOptions,
InteractionOptions
)
from intera_motion_msgs.msg import TrajectoryOptions
from intera_interface import Limb
from intera_motion_interface.utility_functions import int2bool
def interaction_joint_trajectory(limb,
joint_angles,
trajType,
interaction_active,
interaction_control_mode,
interaction_frame,
speed_ratio,
accel_ratio,
K_impedance,
max_impedance,
in_endpoint_frame,
force_command,
K_nullspace,
endpoint_name,
timeout,
disable_damping_in_force_control,
disable_reference_resetting,
rotations_for_constrained_zeroG):
try:
traj = MotionTrajectory(limb=limb)
wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=speed_ratio,
max_joint_accel=accel_ratio)
waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)
# one single waypoint
current_joint_angles = limb.joint_ordered_angles()
waypoint.set_joint_angles(joint_angles = current_joint_angles )
traj.append_waypoint(waypoint.to_msg())
if len(current_joint_angles ) != len(joint_angles):
rospy.logerr('The number of joint_angles must be %d', len(current_joint_angles ))
return None
# ----- testing intermediate points with real robot
middle_joint_angles = [ (current_joint_angles[i] + joint_angles[i])/2.0 for i in xrange(len(current_joint_angles))]
waypoint.set_joint_angles(joint_angles=middle_joint_angles)
traj.append_waypoint(waypoint.to_msg())
waypoint.set_joint_angles(joint_angles = joint_angles)
traj.append_waypoint(waypoint.to_msg())
# ----- end testing intermediate points with real robot
# set the interaction control options in the current configuration
interaction_options = InteractionOptions()
trajectory_options = TrajectoryOptions()
trajectory_options.interaction_control = True
trajectory_options.interpolation_type = trajType
interaction_options.set_interaction_control_active(int2bool(interaction_active))
interaction_options.set_K_impedance(K_impedance)
interaction_options.set_max_impedance(int2bool(max_impedance))
interaction_options.set_interaction_control_mode(interaction_control_mode)
interaction_options.set_in_endpoint_frame(int2bool(in_endpoint_frame))
interaction_options.set_force_command(force_command)
interaction_options.set_K_nullspace(K_nullspace)
interaction_options.set_endpoint_name(endpoint_name)
if len(interaction_frame) < 7:
rospy.logerr('The number of elements must be 7!')
elif len(interaction_frame) == 7:
quat_sum_square = interaction_frame[3]*interaction_frame[3] + interaction_frame[4]*interaction_frame[4] + interaction_frame[5]*interaction_frame[5] + interaction_frame[6]*interaction_frame[6]
if quat_sum_square < 1.0 + 1e-7 and quat_sum_square > 1.0 - 1e-7:
target_interaction_frame = Pose()
target_interaction_frame.position.x = interaction_frame[0]
target_interaction_frame.position.y = interaction_frame[1]
target_interaction_frame.position.z = interaction_frame[2]
target_interaction_frame.orientation.w = interaction_frame[3]
target_interaction_frame.orientation.x = interaction_frame[4]
target_interaction_frame.orientation.y = interaction_frame[5]
target_interaction_frame.orientation.z = interaction_frame[6]
interaction_options.set_interaction_frame(target_interaction_frame)
else:
rospy.logerr('Invalid input to quaternion! The quaternion must be a unit quaternion!')
else:
rospy.logerr('Invalid input to interaction_frame!')
interaction_options.set_disable_damping_in_force_control(disable_damping_in_force_control)
interaction_options.set_disable_reference_resetting(disable_reference_resetting)
interaction_options.set_rotations_for_constrained_zeroG(rotations_for_constrained_zeroG)
trajectory_options.interaction_params = interaction_options.to_msg()
traj.set_trajectory_options(trajectory_options)
result = traj.send_trajectory(timeout=timeout)
if result is None:
rospy.logerr('Trajectory FAILED to send!')
return
if result.result:
rospy.loginfo('Motion controller successfully finished the trajectory with interaction options set!')
else:
rospy.logerr('Motion controller failed to complete the trajectory with error %s',
result.errorId)
# print the resultant interaction options
rospy.loginfo('Interaction Options:\n%s', interaction_options.to_msg())
except rospy.ROSInterruptException:
rospy.logerr('Keyboard interrupt detected from the user. %s',
'Exiting before trajectory completion.')
def main():
"""
Move the robot arm to the specified configuration
with the desired interaction control options.
Call using:
$ rosrun intera_examples go_to_joint_angles_in_contact.py [arguments: see below]
-q 0.0 0.0 0.0 0.0 0.0 0.0 0.0
--> Go to joint pose: 0.0 0.0 0.0 0.0 0.0 0.0 0.0 using default settings
-q 0.1 -0.2 0.15 -0.05 -0.08 0.14 -0.04 -s 0.1
--> Go to pose [...] with a speed ratio of 0.1
-q -0.2 0.1 0.1 0.2 -0.3 0.2 0.4 -s 0.9 -a 0.1
--> Go to pose [...] witha speed ratio of 0.9 and an accel ratio of 0.1
--trajType CARTESIAN
--> Use a Cartesian interpolated endpoint path to reach the goal
=== Interaction Mode options ===
-st 1
--> Set the interaction controller state (1 for True, 0 for False) in the current configuration
-k 500.0 500.0 500.0 10.0 10.0 10.0
--> Set K_impedance to [500.0 500.0 500.0 10.0 10.0 10.0] in the current configuration
-m 0
--> Set max_impedance to False for all 6 directions in the current configuration
-m 1 1 0 1 1 1
--> Set max_impedance to [True True False True True True] in the current configuration
-kn 5.0 3.0 5.0 4.0 6.0 4.0 6.0
--> Set K_nullspace to [5.0 3.0 5.0 4.0 6.0 4.0 6.0] in the current configuration
-f 0.0 0.0 30.0 0.0 0.0 0.0
--> Set force_command to [0.0 0.0 30.0 0.0 0.0 0.0] in the current configuration
-ef
--> Set in_endpoint_frame to True in the current configuration
-en 'right_hand'
--> Specify the desired endpoint frame where impedance and force control behaviors are defined
-md 1
--> Set interaction_control_mode to impedance mode for all 6 directions in the current configuration
(1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit)
-md 1 1 2 1 1 1
--> Set interaction_control_mode to [impedance, impedance, force, impedance, impedance, impedance] in the current configuration
(1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit)
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument(
"-q", "--joint_angles", type=float,
nargs='+', default=[0.0, -0.9, 0.0, 1.8, 0.0, -0.9, 0.0],
help="A list of joint angles, one for each of the 7 joints, J0...J6")
parser.add_argument(
"-s", "--speed_ratio", type=float, default=0.1,
help="A value between 0.001 (slow) and 1.0 (maximum joint velocity)")
parser.add_argument(
"-a", "--accel_ratio", type=float, default=0.5,
help="A value between 0.001 (slow) and 1.0 (maximum joint accel)")
parser.add_argument(
"-t", "--trajType", type=str, default='JOINT',
choices=['JOINT', 'CARTESIAN'],
help="trajectory interpolation type")
parser.add_argument(
"-st", "--interaction_active", type=int, default=1, choices = [0, 1],
help="Activate (1) or Deactivate (0) interaction controller")
parser.add_argument(
"-k", "--K_impedance", type=float,
nargs='+', default=[1300.0, 1300.0, 1300.0, 30.0, 30.0, 30.0],
help="A list of desired stiffnesses, one for each of the 6 directions -- stiffness units are (N/m) for first 3 and (Nm/rad) for second 3 values")
parser.add_argument(
"-m", "--max_impedance", type=int,
nargs='+', default=[1, 1, 1, 1, 1, 1], choices = [0, 1],
help="A list of impedance modulation state, one for each of the 6 directions (a single value can be provided to apply the same value to all the directions) -- 0 for False, 1 for True")
parser.add_argument(
"-md", "--interaction_control_mode", type=int,
nargs='+', default=[1, 1, 1, 1, 1, 1], choices = [1,2,3,4],
help="A list of desired interaction control mode (1: impedance, 2: force, 3: impedance with force limit, 4: force with motion limit), one for each of the 6 directions")
parser.add_argument(
"-fr", "--interaction_frame", type=float,
nargs='+', default=[0, 0, 0, 1, 0, 0, 0],
help="Specify the reference frame for the interaction controller -- first 3 values are positions [m] and last 4 values are orientation in quaternion (w, x, y, z)")
parser.add_argument(
"-ef", "--in_endpoint_frame", action='store_true', default=False,
help="Set the desired reference frame to endpoint frame; otherwise, it is base frame by default")
parser.add_argument(
"-en", "--endpoint_name", type=str, default='right_hand',
help="Set the desired endpoint frame by its name; otherwise, it is right_hand frame by default")
parser.add_argument(
"-f", "--force_command", type=float,
nargs='+', default=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
help="A list of desired force commands, one for each of the 6 directions -- in force control mode this is the vector of desired forces/torques to be regulated in (N) and (Nm), in impedance with force limit mode this vector specifies the magnitude of forces/torques (N and Nm) that the command will not exceed")
parser.add_argument(
"-kn", "--K_nullspace", type=float,
nargs='+', default=[5.0, 10.0, 5.0, 10.0, 5.0, 10.0, 5.0],
help="A list of desired nullspace stiffnesses, one for each of the 7 joints (a single value can be provided to apply the same value to all the directions) -- units are in (Nm/rad)")
parser.add_argument(
"-dd", "--disable_damping_in_force_control", action='store_true', default=False,
help="Disable damping in force control")
parser.add_argument(
"-dr", "--disable_reference_resetting", action='store_true', default=False,
help="The reference signal is reset to actual position to avoid jerks/jumps when interaction parameters are changed. This option allows the user to disable this feature.")
parser.add_argument(
"-rc", "--rotations_for_constrained_zeroG", action='store_true', default=False,
help="Allow arbitrary rotational displacements from the current orientation for constrained zero-G (use only for a stationary reference orientation)")
parser.add_argument(
"--timeout", type=float, default=None,
help="Max time in seconds to complete motion goal before returning. None is interpreted as an infinite timeout.")
args = parser.parse_args(rospy.myargv()[1:])
rospy.init_node('go_to_joint_angles_in_contact_py')
limb = Limb()
interaction_joint_trajectory(limb,
joint_angles = args.joint_angles,
trajType= args.trajType,
interaction_control_mode= args.interaction_control_mode,
interaction_active= args.interaction_active,
interaction_frame= args.interaction_frame,
speed_ratio = args.speed_ratio,
accel_ratio = args.accel_ratio,
K_impedance= args.K_impedance,
max_impedance = args.max_impedance,
in_endpoint_frame = args.in_endpoint_frame,
force_command = args.force_command,
K_nullspace = args.K_nullspace,
endpoint_name = args.endpoint_name,
timeout = args.timeout,
disable_damping_in_force_control=args.disable_damping_in_force_control,
disable_reference_resetting = args.disable_reference_resetting ,
rotations_for_constrained_zeroG = args.rotations_for_constrained_zeroG
)
if __name__ == '__main__':
main()

166
src/sorting_demo/src/sorting_demo/gripper_action_server.py Executable file
Просмотреть файл

@ -0,0 +1,166 @@
#! /usr/bin/env python
from math import fabs
import rospy
import actionlib
import demo_constants
from control_msgs.msg import (
GripperCommandAction,
GripperCommandFeedback,
GripperCommandResult,
)
import real_gripper
from real_gripper import PSGGripper
import intera_interface
class GripperActionServer(object):
def __init__(self):
self._ns = "/robot/gripper_action_server"
if demo_constants.is_real_robot():
self._gripper = PSGGripper() # intera_interface.Gripper()
else:
self._gripper = intera_interface.Gripper()
# Action Server
self._server = actionlib.SimpleActionServer(
self._ns,
GripperCommandAction,
execute_cb=self._on_gripper_action,
auto_start=False)
self._action_name = rospy.get_name()
self._server.start()
self._gripper.set_dead_zone("0.021")
# Action Feedback/Result
self.feedback_msg = GripperCommandFeedback()
self.result_msg = GripperCommandResult()
# Initialize Parameters
self._timeout = 5.0
def _update_feedback(self, position):
self.feedback_msg.position = self._gripper.get_position()
self.feedback_msg.effort = self._gripper.get_force()
self.feedback_msg.stalled = False
self.feedback_msg.reached_goal = False
self._server.publish_feedback(self.feedback_msg)
def _command_gripper_update(self, position, is_close_goal):
if is_close_goal:
self._gripper.close()
rospy.logwarn("cmd: close")
else:
self._gripper.open()
rospy.logwarn("cmd: open")
# self._gripper.set_position(position)
def check_success(self, position, close_goal):
rospy.logwarn("gripping force: " + str(self._gripper.get_force()))
rospy.logwarn("gripper position: " + str(self._gripper.get_position()))
rospy.logwarn("gripper position deadzone: " + str(self._gripper.get_dead_zone()))
if not self._gripper.is_moving():
success = True
else:
success = False
# success = fabs(self._gripper.get_position() - position) < self._gripper.get_dead_zone()
rospy.logwarn("gripping success: " + str(success))
return success
def _on_gripper_action(self, goal):
# Store position and effort from call
# Position to 0:100 == close:open
position = goal.command.position
effort = goal.command.max_effort
# Check for errors
if self._gripper.has_error():
rospy.logerr("%s: Gripper error - please restart action server." %
(self._action_name,))
self._server.set_aborted()
is_close_goal = position < 0.02
# Reset feedback/result
self._update_feedback(position)
# 20 Hz gripper state rate
control_rate = rospy.Rate(20.0)
# Record start time
start_time = rospy.get_time()
def now_from_start(start):
return rospy.get_time() - start
self._command_gripper_update(position, is_close_goal)
rospy.sleep(0.2)
# Continue commanding goal until success or timeout
while ((now_from_start(start_time) < self._timeout or
self._timeout < 0.0) and not rospy.is_shutdown()):
if self._server.is_preempt_requested():
self._gripper.stop()
rospy.loginfo("%s: Gripper Action Preempted" %
(self._action_name,))
self._server.set_preempted(self.result_msg)
return
self._update_feedback(position)
if self.check_success(position, is_close_goal):
self._server.set_succeeded(self.result_msg)
return
self._command_gripper_update(position, is_close_goal)
control_rate.sleep()
# Gripper failed to achieve goal before timeout/shutdown
self._gripper.stop()
if not rospy.is_shutdown():
rospy.logerr("%s: Gripper Command Not Achieved in Allotted Time" %
(self._action_name,))
self._update_feedback(position)
self._server.set_aborted(self.result_msg)
import argparse
import rospy
from dynamic_reconfigure.server import Server
def start_server(gripper):
print("Initializing node... ")
rospy.init_node("rsdk_gripper_action_server%s" %
("" if gripper == 'both' else "_" + gripper,))
print("Initializing gripper action server...")
gas = GripperActionServer()
print("Running. Ctrl-c to quit")
rospy.spin()
def main():
arg_fmt = argparse.ArgumentDefaultsHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt)
parser.add_argument("-g", "--gripper", dest="gripper", default="both",
choices=['both', 'left', 'right'],
help="gripper action server limb", )
args = parser.parse_args(rospy.myargv()[1:])
start_server(args.gripper)
if __name__ == "__main__":
main()

58
src/sorting_demo/src/sorting_demo/program.py Executable file
Просмотреть файл

@ -0,0 +1,58 @@
#!/usr/bin/python
import functools
import sys
import rospy
from task_planner import TaskPlanner
import demo_constants
import gazebo_models
def main():
rospy.init_node("sorting_demo")
rospy.logwarn("Hello world")
if not demo_constants.is_real_robot():
# Load Gazebo Models via Spawning Services
# Note that the models reference is the /world frame
# and the IK operates with respect to the /base frame
model_list, original_block_poses = gazebo_models.load_gazebo_models()
# Remove models from the scene on shutdown
rospy.on_shutdown(functools.partial(gazebo_models.delete_gazebo_models, model_list))
rospy.logwarn("Creating task planner")
task_planner = TaskPlanner()
rospy.logwarn("Hello world")
task_planner.create_go_home_task(check_obstacles=False).result()
task_facade = task_planner.get_task_facade()
task_facade.start()
task_facade.run_rest_server()
# rospy.sleep(15)
# task_facade.pick_block_by_color("BLUE")
# task_facade.put_block_into_tray("BLUE", "1")
# task_facade.put_all_contents_on_table()
# task_facade.pause()
# rospy.sleep(30)
# task_facade.resume()
# task_facade.stop("GREEN")
rospy.logwarn("task planner spin")
task_planner.spin()
# ask the robot to greet
# task_facade.greet()
if __name__ == '__main__':
sys.exit(main())

32
src/sorting_demo/src/sorting_demo/real_gripper.py Normal file
Просмотреть файл

@ -0,0 +1,32 @@
class PSGGripper():
def __init__(self):
pass
def get_position(self):
return 0
def get_force(self):
return 0
def set_dead_zone(self, v):
return 0
def get_dead_zone(self):
return 0
def is_moving(self):
return 0
def has_error(self):
return False
def stop(self):
pass
def close(self):
pass
def open(self):
pass

62
src/sorting_demo/src/sorting_demo/reinforcement_learning/task_space_simulation.py Normal file
Просмотреть файл

@ -0,0 +1,62 @@
import gym
from gym import utils
from gym.envs.toy_text import discrete
import numpy as np
class PositionEnv(discrete.DiscreteEnv):
"""
Grid 5x5 world: 25 slots 4 posibles values (B,G,R,-)
25^4 = 390625 states (390K)
Actions: 25 (pick or place)
The action policy is going to be: 390K
"""
def __init__(self, nS, nA, P, isd):
super(PositionEnv, self).__init__(nS, nA, P, isd)
self.desc = np.array()
self.rows = 5
self.cols = 5
self.values = 3 + 1
self.states = (self.rows*self.cols)^(self.values)
self.actions = self.rows* self.cols
# transition model
# {action: [(probability, nextstate, reward, done)]}
self.P = {s: {a: [] for a in range(self.actions)} for s in range(self.states)}
def is_holand_flag(self, ):
pass
def encode(self):
pass
def step(self,action):
pass
def render(self):
pass
def encode_state_aux(i, values, slots):
if slots == 1:
return [i]
else:
current = i % values
return encode_state_aux(i / values, values, slots - 1) + [current]
slots = 16
values = 4
count = values**slots
for i in xrange(count):
print str(i)+ ":" + str(encode_state_aux(i, values, slots))

103
src/sorting_demo/src/sorting_demo/robot_control.py Executable file
Просмотреть файл

@ -0,0 +1,103 @@
#!/usr/bin/python
import copy
from datetime import time
import intera_interface
import rospy
from geometry_msgs.msg import (
Pose,
)
from sorting_demo import demo_constants
from real_gripper import PSGGripper
class SawyerRobotControl(object):
def __init__(self, trajectory_planner, limb="right", hover_distance=0.08, tip_name="right_gripper_tip"):
"""
:param limb:
:param hover_distance:
:param tip_name:
"""
self.trajectory_planner = trajectory_planner
self._limb_name = limb # string
self._tip_name = tip_name # string
self._hover_distance = hover_distance # in meters
self._limb = intera_interface.Limb(limb)
if demo_constants.is_real_robot():
self._gripper =PSGGripper()
else:
self._gripper = intera_interface.Gripper()
self._robot_enable = intera_interface.RobotEnable()
# verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
"""
def move_to_start(self, start_angles=None):
'''
:param start_angles:
:return:
'''
print("Moving the {0} arm to start pose...".format(self._limb_name))
if not start_angles:
start_angles = dict(zip(self._joint_names, [0] * 7))
self._guarded_move_to_joint_position(start_angles)
self.gripper_open()
"""
def gripper_open(self):
"""
:return:
"""
rospy.logwarn("OPENING GRIPPER")
self._gripper.open()
while self._gripper.is_moving() and not rospy.is_shutdown():
rospy.sleep(0.4)
def gripper_close(self):
"""
:return:
"""
rospy.logwarn("CLOSING GRIPPER")
self._gripper.close()
while self._gripper.is_moving() and not rospy.is_shutdown():
rospy.sleep(0.1)
def _guarded_move_to_joint_position(self, joint_angles, timeout=5.0):
"""
:param joint_angles:
:param timeout:
:return:
"""
if rospy.is_shutdown():
return
if joint_angles:
self._limb.move_to_joint_positions(joint_angles, timeout=timeout)
else:
rospy.logerr("No Joint Angles provided for move_to_joint_positions. Staying put.")
def disable(self):
"""
:return:
"""
self._robot_enable.disable()
def enable(self):
"""
:return:
"""
self._robot_enable.enable()

18
src/sorting_demo/src/sorting_demo/robot_funcs.py Normal file
Просмотреть файл

@ -0,0 +1,18 @@
import math
import rospy
def force_joint_limits(jntspos):
joint_limits = [(-3.0503, 3.0503), (-3.8095, 2.2736), (-3.0426, 3.0426), (-3.0439, 3.0439)]
rospy.logwarn(jntspos)
for i in xrange(len(joint_limits)):
lower = joint_limits[i][0]
upper = joint_limits[i][1]
if jntspos[i] < lower and (jntspos[i] + 2.0 * math.pi) < upper:
jntspos[i] = jntspos[i] + 2 * math.pi
if jntspos[i] > upper and (jntspos[i] - 2.0 * math.pi) > lower:
jntspos[i] = jntspos[i] - 2 * math.pi
return jntspos

266
src/sorting_demo/src/sorting_demo/robot_tasks_facade.py Normal file
Просмотреть файл

@ -0,0 +1,266 @@
import flask
import rospy
from flask import Flask
class RobotTaskFacade:
"""
class specially designed to control the robot via voice commands. Some examples could be:
'Hello robot'
'Hey robot! What task are you doing now?'
'Hey robot! Put all red pieces on the tray 2'
'Hey robot! Pause your work'
'Hey robot! Give me a red brick'
'Hey robot! Stop sorting green pieces'
'Hey robot! restart your work'
'Hey robot! What is the color of the piece you are grabbing'
'Hey robot! Put all tray contents on the table'
"""
def __init__(self, task_planner):
self.task_planner = task_planner
self.app = Flask(__name__)
@self.app.route("/state")
def get_state():
try:
return flask.json.jsonify(response="ACK", result=self.get_state())
except Exception as ex:
return flask.json.jsonify(response="ERR", message=ex.message)
@self.app.route("/current_task")
def get_current_task():
try:
return flask.json.jsonify(response= "ACK", result = self.get_current_task())
except Exception as ex:
return flask.json.jsonify(response= "ERR", message = ex.message)
@self.app.route("/count_table_pieces/<color>")
def count_pieces_on_table_by_color(color):
try:
return flask.json.jsonify(response="ACK", result=self.count_pieces_on_table_by_color(color))
except Exception as ex:
return flask.json.jsonify(response="ERR", message=ex.message)
@self.app.route("/current_piece_color")
def get_current_piece_color():
try:
return flask.json.jsonify(response="ACK", result=self.get_current_piece_color())
except Exception as ex:
return flask.json.jsonify(response="ERR", message=ex.message)
@self.app.route("/pause")
def pause():
try:
return flask.json.jsonify(response="ACK", result=self.pause())
except Exception as ex:
return flask.json.jsonify(response="ERR", message=ex.message)
@self.app.route("/resume")
def resume():
try:
return flask.json.jsonify(response="ACK", result=self.resume())
except Exception as ex:
return flask.json.jsonify(response="ERR", message=ex.message)
@self.app.route("/start")
def start():
try:
return flask.json.jsonify(response="ACK", result=self.start())
except Exception as ex:
return flask.json.jsonify(response="ERR", message=ex.message)
@self.app.route("/stop")
def stop():
try:
return flask.json.jsonify(response="ACK", result=self.stop())
except Exception as ex:
return flask.json.jsonify(response="ERR", message=ex.message)
@self.app.route("/greet")
def greet():
try:
return flask.json.jsonify(response="ACK", result=self.greet())
except Exception as ex:
return flask.json.jsonify(response="ERR", message=ex.message)
@self.app.route("/put_all_contents_on_table")
def put_all_contents_on_table():
try:
return flask.json.jsonify(response="ACK", result=self.put_all_contents_on_table())
except Exception as ex:
return flask.json.jsonify(response="ERR", message=ex.message)
@self.app.route("/pick_block_by_color/<color>")
def pick_block_by_color(color):
try:
return flask.json.jsonify(response="ACK", result=self.pick_block_by_color(color))
except Exception as ex:
return flask.json.jsonify(response="ERR", message=ex.message)
@self.app.route("/place_block_to_tray/<trayid>")
def place_block_to_tray_id(trayid):
try:
return flask.json.jsonify(response="ACK", result=self.place_block_to_tray(int(trayid)))
except Exception as ex:
return flask.json.jsonify(response="ERR", message=ex.message)
@self.app.route("/put_block_into_tray/<color>/<trayid>")
def put_block_into_tray( color, trayid):
try:
return flask.json.jsonify(response="ACK", result=self.put_block_into_tray(color,int(trayid)))
except Exception as ex:
return flask.json.jsonify(response="ERR", message=ex.message)
@self.app.route("/view")
def web_interface():
import rospkg
webviewpath = rospkg.RosPack().get_path('sorting_demo') + "/share/web_view.html"
with open(webviewpath, 'r') as myfile:
data = myfile.read()
return data
def run_rest_server(self):
self.app.run(threaded=True)
# -------- observer methods ------------------
def get_state(self):
"""
:return: the state of the application
"""
return self.task_planner.get_state()
def get_current_task(self):
"""
:return: str - it gets the name of the task the robot is doing
"""
# returns the higher level task
task_stack = self.task_planner.get_task_stack()
return task_stack
def count_pieces_on_table_by_color(self, color):
"""
:param color: str - "red", "blue", "green"
:return:
"""
count = self.task_planner.count_pieces_on_table_by_color(color)
return count
def get_current_piece_color(self):
"""
:return:
"""
return None if self.task_planner.gripper_state.holding_block is None else self.task_planner.gripper_state.holding_block.get_color()
# ---------- lifecycle ---------------------------
def pause(self):
"""
'Hey robot! Stop your work'
It pauses the loop job
:return:
"""
self.task_planner.pause()
return "ACK"
def resume(self):
"""
resume the loop job
:return:
"""
self.task_planner.resume()
return "ACK"
def start(self):
"""
:return:
"""
self.task_planner.execute_task(self.task_planner.create_main_loop_task)
return "ACK"
def stop(self):
"""
:return:
"""
self.task_planner.stop()
return "ACK"
# --------- request tasks methods ---------------------
def greet(self):
"""
'Hello robot'
:return:
"""
self.task_planner.execute_task(self.task_planner.create_greet_task)
return "ACK"
def put_all_contents_on_table(self):
"""
'Hey robot! Put all red pieces on the tray 2'
:return:
"""
self.task_planner.execute_task(self.task_planner.put_all_contents_on_table)
return "ACK"
def pick_block_by_color(self, color):
"""
'Hey robot! Give me a red brick'
:param color: str - "red", "blue", "green"
:return:
"""
self.task_planner.execute_task(self.task_planner.pick_block_from_table_by_color, args=[color])
return "ACK"
def place_block_to_tray(self, trayid):
self.task_planner.execute_task(self.task_planner.place_block_to_tray, args=[trayid])
return "ACK"
def put_block_into_tray(self, color, trayid):
"""
'Hey robot! Put a red piece on the tray 2'
:param color: str - "red", "blue", "green"
:param tray:
:return:
"""
self.task_planner.execute_task(self.task_planner.put_block_into_tray_task, args=[color,trayid])
return "ACK"
# ------------ configure loop sorting ---------------------
#@route("/disable_sorting_by_color/<color>')")
def disable_sorting_by_color(self, color):
"""
'Hey robot! Stop sorting green pieces'
:param color:
:return:
"""
self.task_planner.execute_task(lambda: self.task_planner.disable_sorting_by_color(color))
#@route("/enable_sorting_by_color/<color>')")
def enable_sorting_by_color(self,color):
"""
'Hey robot! Stop sorting green pieces'
:param color:
:return:
"""
self.task_planner.execute_task(lambda: self.task_planner.enable_sorting_by_color(color))

1362
src/sorting_demo/src/sorting_demo/task_planner.py Normal file
Просмотреть файл

Разница между файлами не показана из-за своего большого размера Загрузить разницу

0
src/sorting_demo/src/sorting_demo/tasks/__init__.py Normal file
Просмотреть файл

80
src/sorting_demo/src/sorting_demo/tasync.py Normal file
Просмотреть файл

@ -0,0 +1,80 @@
from functools import wraps
import rospy
import traceback
import sys
def tasync(taskname):
def wrapper(f):
@wraps(f)
def wrapped(self, *f_args, **f_kwargs):
if rospy.is_shutdown():
sys.exit(0)
if self.has_cancel_signal():
raise Exception("canceling")
rospy.logerr("trying to invoke but cancel signal: " + str(taskname))
self.print_tasks()
return Task("CANCEL", None)
if self.pause_flag:
rospy.logerr("PAUSE")
while self.pause_flag and not rospy.is_shutdown():
rospy.sleep(0.5)
rospy.logwarn("Task %s is paused" % taskname)
tt = Task(taskname, None)
def lamb():
res = None
try:
# f_kwargs["task"] = tt
rospy.logwarn("launching task")
res = f(self, *f_args, **f_kwargs)
except Exception as ex:
rospy.logerr("task wrapping error (%s): %s" % (taskname, str(ex)))
traceback.print_exc()
return res
self.add_task(tt)
rospy.logwarn("launching task")
fut = self.executor.submit(lamb)
tt.future = fut
def got_result(fut):
try:
rospy.logwarn("removing task: " + tt.name)
self.remove_task(tt)
except Exception as ex:
rospy.logwarn("error at done callback: " + tt.name + str(ex))
self.print_tasks()
fut.add_done_callback(got_result)
return tt
return wrapped
return wrapper
class Task:
def __init__(self, name, future):
self.name = name
self.future = future
self.marked_cancel = False
self.resultcancel = None
def cancel(self):
self.marked_cancel = True
self.resultcancel = self.future.cancel()
def result(self):
if self.future is not None:
return self.future.result()
else:
return None

493
src/sorting_demo/src/sorting_demo/trajectory_planner.py Executable file
Просмотреть файл

@ -0,0 +1,493 @@
#! /usr/bin/env python
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import trajectory_msgs.msg
from geometry_msgs.msg import Point, Pose
import demo_constants
class TrajectoryPlanner:
def __init__(self):
"""
"""
self.ceilheight = 0.75
rospy.sleep(0.4)
moveit_commander.roscpp_initialize(sys.argv)
rospy.sleep(0.4)
self.scene = moveit_commander.PlanningSceneInterface()
self.robot = moveit_commander.RobotCommander()
rospy.sleep(0.1)
self.group = moveit_commander.MoveGroupCommander("right_arm")
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory)
self.planning_scene_diff_publisher = rospy.Publisher("planning_scene", moveit_msgs.msg.PlanningScene,
queue_size=1)
rospy.sleep(0.1)
self.set_default_planner()
print "============ Reference frame: %s" % self.group.get_planning_frame()
print "============ Reference frame: %s" % self.group.get_end_effector_link()
print self.robot.get_group_names()
print self.robot.get_current_state()
self.enable_collision_table1 = True
self.enable_orientation_constraint = False
self.set_default_tables_z()
self.registered_blocks = []
self.tableshape = (0.913, 0.913, 0.01)
# self.tableshape = (1.2, 1.2, 0.01)
rospy.sleep(0.2)
def register_box(self, block):
if not block in self.registered_blocks:
block.perception_id = "block"+ str(len(self.registered_blocks))
self.registered_blocks.append(block)
rospy.sleep(0.1)
def set_default_tables_z(self):
self.table1_z = demo_constants.TABLE_HEIGHT + 0.02
self.table2_z = demo_constants.TABLE_HEIGHT + 0.05
def set_default_planner(self):
# self.group.set_planner_id("OMPL")
#self.group.set_planner_id("RRTConnectkConfigDefault")
# self.group.set_planner_id("LBKPIECEkConfigDefault")
self.group.set_planner_id("ESTkConfigDefault")
# commit
rospy.sleep(0.2)
def pick_block(self, block, surface):
self.group.set_num_planning_attempts(10)
self.group.set_planning_time(3)
#pathconstraint = self.joint_constraints()
#self.group.set_path_constraints(pathconstraint)
target_pose = block.grasp_pose
self.update_environment_obstacles()
block_index = self.update_block(block)
self.group.set_support_surface_name(surface)
grasp = moveit_msgs.msg.Grasp()
grasp.grasp_pose.header.frame_id = "world"
grasp.grasp_pose.header.stamp = rospy.Time.now()
grasp.grasp_pose.pose = target_pose
grasp.grasp_pose.pose.position.z += 0.1
grasp.pre_grasp_approach.direction.vector.z = -1.0
grasp.pre_grasp_approach.min_distance = 0.08
grasp.pre_grasp_approach.desired_distance = 0.1
# opened gripper
grasp.pre_grasp_posture.joint_names = ["right_gripper_l_finger_joint", "right_gripper_r_finger_joint"]
trajpoint = trajectory_msgs.msg.JointTrajectoryPoint()
trajpoint.positions = [0.04, 0.04]
trajpoint.time_from_start = rospy.Duration(0.5)
grasp.pre_grasp_posture.points = [trajpoint]
grasp.post_grasp_retreat.direction.vector.z = 1.0
grasp.post_grasp_retreat.min_distance = 0.08
grasp.post_grasp_retreat.desired_distance = 0.1
# closed gripper
grasp.grasp_posture.joint_names = ["right_gripper_l_finger_joint", "right_gripper_r_finger_joint"]
trajpoint = trajectory_msgs.msg.JointTrajectoryPoint()
trajpoint.positions = [0.0, 0.0]
trajpoint.time_from_start = rospy.Duration(0.5)
grasp.grasp_posture.points = [trajpoint]
graps = [grasp]
rospy.logwarn("blocks that can be picked: " + str(self.registered_blocks))
return self.group.pick("block" + str(block_index), graps)
def place_block(self, block):
self.group.set_num_planning_attempts(10)
self.group.set_planning_time(3)
self.update_environment_obstacles()
place_location = moveit_msgs.msg.PlaceLocation()
place_location.place_pose.header.frame_id = "world"
place_location.place_pose.pose = block.place_pose
place_location.pre_place_approach.direction.header.frame_id = "world"
place_location.pre_place_approach.direction.vector.z = -1.0
place_location.pre_place_approach.min_distance = 0.08
place_location.pre_place_approach.desired_distance = 0.1
place_location.post_place_retreat.direction.header.frame_id = "world"
place_location.post_place_retreat.direction.vector.z = 1.0
place_location.post_place_retreat.min_distance = 0.08
place_location.post_place_retreat.desired_distance = 0.1
# open
place_location.post_place_posture.joint_names = ["right_gripper_l_finger_joint", "right_gripper_r_finger_joint"]
trajpoint = trajectory_msgs.msg.JointTrajectoryPoint()
trajpoint.positions = [0.04, 0.04]
trajpoint.time_from_start = rospy.Duration(0.5)
place_location.post_place_posture.points = [trajpoint]
self.group.set_support_surface_name("table2")
rospy.sleep(1.0)
return self.group.place("block" + str(self.registered_blocks.index(block)), place_location)
def update_block(self, block):
rospy.logwarn("updating block collision info")
if not block in self.registered_blocks:
raise Exception("block does not exist")
rospy.logwarn("current blocks: " + str(self.registered_blocks))
block_pose = geometry_msgs.msg.PoseStamped()
block_pose.pose = block.grasp_pose
block_pose.header.stamp = rospy.Time.now()
block_pose.header.frame_id = self.robot.get_planning_frame()
block_index = self.registered_blocks.index(block)
self.scene.add_box("block" + str(block_index), block_pose, size=(0.04, 0.04, 0.04))
rospy.sleep(0.5)
return block_index
def update_table1_collision(self):
table1pose = geometry_msgs.msg.PoseStamped()
table1pose.pose = Pose(position=Point(x=0.75, y=0.0, z=self.table1_z))
table1pose.pose.orientation.w = 1.0
table1pose.header.stamp = rospy.Time.now()
table1pose.header.frame_id = "world"
self.scene.add_box("table1", table1pose, size=self.tableshape)
self.update_table_edges_collision(table1pose, 0.15,"table1",self.table1_z)
def update_table2_collision(self):
table2pose = geometry_msgs.msg.PoseStamped()
table2pose.pose = Pose(position=Point(x=0.0, y=1.0, z=self.table2_z))
table2pose.pose.orientation.w = 1.0
table2pose.header.stamp = rospy.Time.now()
table2pose.header.frame_id = "world"
edgeheight = 0.15
self.scene.add_box("table2", table2pose, size=self.tableshape)
self.update_table_edges_collision(table2pose, edgeheight,"table2",self.table2_z)
splits = 3
for i in xrange(1, splits):
edgeyi = geometry_msgs.msg.PoseStamped()
edgeyi.pose = Pose(
position=Point(x=i * self.tableshape[0] / float(splits) - self.tableshape[0] * 0.5, y=1.0,
z=self.table2_z + edgeheight * 0.5))
edgeyi.pose.orientation.w = 1.0
edgeyi.header.stamp = rospy.Time.now()
edgeyi.header.frame_id = "world"
newshape = (
self.tableshape[0] * 0.01, self.tableshape[1], self.tableshape[2] + edgeheight * 0.5)
self.scene.add_box("table2_edgey_" + str(i), edgeyi, size=newshape)
splits = 2
for i in xrange(1, splits):
edgexi = geometry_msgs.msg.PoseStamped()
edgexi.pose = Pose(
position=Point(x= 0.0, y=1.0 - self.tableshape[1] * 0.5 + i * self.tableshape[1] / float(splits), z=self.table2_z + edgeheight * 0.5))
edgexi.pose.orientation.w = 1.0
edgexi.header.stamp = rospy.Time.now()
edgexi.header.frame_id = "world"
newshape = (
self.tableshape[0] , self.tableshape[1] * 0.01, self.tableshape[2] + edgeheight * 0.5)
self.scene.add_box("table2_edgex_" + str(i), edgexi, size=newshape)
def update_table_edges_collision(self, basepose, edgeheight,tablename, table_z):
edgex = copy.deepcopy(basepose)
edgex.pose.position.x += 0.0
edgex.pose.position.y += -self.tableshape[1] / 2.0
edgex.pose.position.z= table_z + edgeheight * 0.5
edgex.pose.orientation.w = 1.0
edgex.header.stamp = rospy.Time.now()
edgex.header.frame_id = "world"
newshape = (self.tableshape[0], self.tableshape[1] * 0.01, self.tableshape[2] + edgeheight)
self.scene.add_box("%s_edgex"%tablename, edgex, size=newshape)
edgex2 = copy.deepcopy(basepose)
edgex2.pose.position.x+=0.0
edgex2.pose.position.y+= self.tableshape[1] / 2.0
edgex2.pose.position.z= table_z+ edgeheight * 0.5
edgex2.pose.orientation.w = 1.0
edgex2.header.stamp = rospy.Time.now()
edgex2.header.frame_id = "world"
newshape = (self.tableshape[0], self.tableshape[1] * 0.01, self.tableshape[2] + edgeheight)
self.scene.add_box("%s_edgex2"%tablename, edgex2, size=newshape)
edgey = copy.deepcopy(basepose)
edgey.pose.position.x+=self.tableshape[0] / 2.0
edgey.pose.position.y+= 0
edgey.pose.position.z=table_z + edgeheight * 0.5
edgey.pose.orientation.w = 1.0
edgey.header.stamp = rospy.Time.now()
edgey.header.frame_id = "world"
newshape = (self.tableshape[0] * 0.01, self.tableshape[1], self.tableshape[2] + edgeheight)
self.scene.add_box("%s_edgey"%tablename, edgey, size=newshape)
edgey2 = copy.deepcopy(basepose)
edgey2.pose.position.x+=- self.tableshape[0] / 2.0
edgey2.pose.position.y +=0
edgey2.pose.position.z=table_z + edgeheight * 0.5
edgey2.pose.orientation.w = 1.0
edgey2.header.stamp = rospy.Time.now()
edgey2.header.frame_id = "world"
newshape = (self.tableshape[0] * 0.01, self.tableshape[1], self.tableshape[2] + edgeheight * 0.5)
self.scene.add_box("%s_edgey2"%tablename, edgey2, size=newshape)
def update_ceiling_obstacle(self):
ceilshape = (2.0, 2.0, 0.02)
ceil1pose = geometry_msgs.msg.PoseStamped()
ceil1pose.pose = Pose(position=Point(x=0.0, y=0.0, z=self.ceilheight))
ceil1pose.pose.orientation.w = 1.0
ceil1pose.header.stamp = rospy.Time.now()
ceil1pose.header.frame_id = self.robot.get_planning_frame()
self.scene.add_box("ceil1", ceil1pose, size=ceilshape)
def update_environment_obstacles(self):
"""
:return:
"""
backwall = (0.01, 0.6, 1.4)
largexwall = (0.01, 2.0, 1.6)
ywall = (2.0, 0.04, 1.6)
self.update_ceiling_obstacle()
xwallpose = geometry_msgs.msg.PoseStamped()
xwallpose.pose = Pose(position=Point(x=-0.4, y=-0.25, z=0.0))
xwallpose.pose.orientation.w = 1.0
xwallpose.header.stamp = rospy.Time.now()
xwallpose.header.frame_id = self.robot.get_planning_frame()
self.scene.add_box("backwall", xwallpose, size=backwall)
postxwallpose = geometry_msgs.msg.PoseStamped()
postxwallpose.pose = Pose(position=Point(x=-0.5, y=0.0, z=0.0))
postxwallpose.pose.orientation.w = 1.0
postxwallpose.header.stamp = rospy.Time.now()
postxwallpose.header.frame_id = self.robot.get_planning_frame()
self.scene.add_box("postbackwall", postxwallpose, size=largexwall)
frontwall = geometry_msgs.msg.PoseStamped()
frontwall.pose = Pose(position=Point(x=1.1, y=0.0, z=0.0))
frontwall.pose.orientation.w = 1.0
frontwall.header.stamp = rospy.Time.now()
frontwall.header.frame_id = self.robot.get_planning_frame()
self.scene.add_box("frontwall", frontwall, size=largexwall)
traywall = geometry_msgs.msg.PoseStamped()
traywall.pose = Pose(position=Point(x=-0.0, y=1.5, z=0.0))
traywall.pose.orientation.w = 1.0
traywall.header.stamp = rospy.Time.now()
traywall.header.frame_id = self.robot.get_planning_frame()
self.scene.add_box("traywall", traywall, size=ywall)
frontsidewall = geometry_msgs.msg.PoseStamped()
frontsidewall.pose = Pose(position=Point(x=-0.0, y=-0.55, z=0.0))
frontsidewall.pose.orientation.w = 1.0
frontsidewall.header.stamp = rospy.Time.now()
frontsidewall.header.frame_id = self.robot.get_planning_frame()
self.scene.add_box("frontsidewall", frontsidewall, size=ywall)
rospy.sleep(0.1)
def clear_attached_object(self, block):
self.group.detach_object(block.perception_id)
def joint_constraints(self):
constraints = moveit_msgs.msg.Constraints()
jcm = moveit_msgs.msg.JointConstraint()
jcm.joint_name = "right_j0"
jcm.position = 0
jcm.tolerance_above = 1.6
jcm.tolerance_below = 1.6
jcm.weight = 0.5
constraints.joint_constraints.append(jcm)
return constraints
def path_constraints(self, target_pose):
"""
:return:
"""
constraints = moveit_msgs.msg.Constraints()
"""
orientationconstraint = moveit_msgs.msg.OrientationConstraint()
# orientationconstraint.orientation.w=1
orientationconstraint.orientation.x = target_pose.orientation.x
orientationconstraint.orientation.y = target_pose.orientation.y
orientationconstraint.orientation.z = target_pose.orientation.z
orientationconstraint.orientation.w = target_pose.orientation.w
orientationconstraint.absolute_x_axis_tolerance = 0.1
orientationconstraint.absolute_y_axis_tolerance = 0.1
orientationconstraint.absolute_z_axis_tolerance = 0.1
orientationconstraint.weight = 1.0
orientationconstraint.link_name = "right_l6"
orientationconstraint.header.frame_id = "world"
orientationconstraint.header.stamp = rospy.Time.now()
if self.enable_orientation_constraint:
constraints.orientation_constraints.append(orientationconstraint)
"""
"""
jcm = moveit_msgs.msg.JointConstraint()
jcm.joint_name = "right_j1"
jcm.position = 0
jcm.tolerance_above = 0.5
jcm.tolerance_below = 0.5
jcm.weight = 0.5
constraints.joint_constraints.append(jcm)
rospy.sleep(0.1)
"""
return constraints
def clear_parameters(self):
"""
:return:
"""
self.group.clear_pose_targets()
self.group.clear_path_constraints()
rospy.sleep(0.5)
def publish_planning_scene(self):
"""
collision_objects = self.scene.get_objects()
planning_scene = moveit_msgs.msg.PlanningScene()
planning_scene.world.collision_objects = collision_objects
planning_scene.is_diff = True
print planning_scene
self.planning_scene_diff_publisher.publish(planning_scene)
"""
def move_to_joint_target(self, joints, currentpose=None, attempts=10):
"""
:param joints: float array
:return:
"""
self.clear_parameters()
# self.create_environment_obstacles()
# self.group.set_goal_joint_tolerance (0.2)
try:
self.group.set_joint_value_target(joints)
except Exception as ex:
pass
self.group.set_num_planning_attempts(attempts)
rospy.sleep(0.3)
if currentpose is None:
self.group.set_start_state_to_current_state()
currentpose = self.group.get_current_pose().pose
else:
# self.group.set_start_state(currentpose)
# rospy.logwarn(self.robot.get_current_state())
# currentpose = self.group.get_current_pose(self.group.get_end_effector_link()).pose
self.group.set_start_state_to_current_state()
constraints = self.path_constraints(currentpose)
self.group.set_path_constraints(constraints)
rospy.sleep(0.1)
plan1 = self.group.plan()
rospy.sleep(0.1)
rospy.logwarn("PLAN: " + str(plan1))
self.publish_planning_scene()
# rospy.logwarn("PLAAAAN!!! "+ str(plan1))
if plan1.joint_trajectory.header.frame_id != "":
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state()
display_trajectory.trajectory.append(plan1)
self.display_trajectory_publisher.publish(display_trajectory)
success = self.group.go(wait=True)
rospy.logwarn("result of the motion: " + str(success))
return success
else:
return False
def move_to_cartesian_target(self, pose_target, attempts=100):
"""
:param pose_target: geometry_msgs/Pose
:return:
"""
self.clear_parameters()
self.update_environment_obstacles()
self.group.set_pose_target(pose_target)
self.group.set_num_planning_attempts(attempts)
rospy.sleep(0.3)
constraints = self.path_constraints(self.group.get_current_pose().pose)
self.group.set_path_constraints(constraints)
plan1 = self.group.plan()
rospy.logwarn(plan1)
rospy.sleep(0.1)
self.publish_planning_scene()
# rospy.logwarn("PLAAAAN!!! "+ str(plan1))
if plan1.joint_trajectory.header.frame_id != "":
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state()
display_trajectory.trajectory.append(plan1)
self.display_trajectory_publisher.publish(display_trajectory)
success = self.group.go(wait=True)
rospy.logwarn("result of the motion: " + str(success))
return success
else:
return False

0
src/sorting_demo/src/sorting_demo/utils/__init__.py Normal file
Просмотреть файл

65
src/sorting_demo/src/sorting_demo/utils/mathutils.py Executable file
Просмотреть файл

@ -0,0 +1,65 @@
import tf
import tf.transformations
import numpy
import geometry_msgs.msg
from geometry_msgs.msg import Pose, Point, Quaternion
def inverse_compose(basehomopose, homopose):
"""
:param basehomopose: 4x4 homogeneous transform matrix
:param homopose: 4x4 homogeneous transform matrix
:return:
"""
return numpy.matmul(numpy.linalg.inv(basehomopose), homopose)
def get_homo_matrix_from_pose_msg(pose, tag=""):
basetrans = tf.transformations.translation_matrix((pose.position.x,
pose.position.y,
pose.position.z))
baserot = tf.transformations.quaternion_matrix((pose.orientation.x,
pose.orientation.y,
pose.orientation.z,
pose.orientation.w))
# rospy.loginfo(tag + " basetrans: " + str(basetrans))
# rospy.loginfo(tag +" baserot: " + str(baserot))
combined = numpy.matmul(basetrans, baserot)
# rospy.loginfo(tag + " combined: " + str(combined))
trans = tf.transformations.translation_from_matrix(combined)
quat = tf.transformations.quaternion_from_matrix(combined)
# rospy.loginfo(tag + " back basetrans: " + str(trans))
# rospy.loginfo(tag +" back baserot: " + str(quat))
return combined
def homotransform_to_pose_msg(homotransform):
trans = tf.transformations.translation_from_matrix(homotransform)
quat = tf.transformations.quaternion_from_matrix(homotransform)
return Pose(
position=Point(x=trans[0], y=trans[1], z=trans[2]),
orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]))
def composition(parent, child):
return numpy.matmul(parent, child)
def rot_y(pitch):
return [[numpy.cos(pitch), 0, numpy.sin(pitch), 0], [0, 1, 0, 0], [-numpy.sin(pitch), 0, numpy.cos(pitch), 0],
[0, 0, 0, 1]]
def rot_z(yaw):
return [[numpy.cos(yaw), -numpy.sin(yaw), 0, 0], [numpy.sin(yaw), numpy.cos(yaw), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]
def rot_x(roll):
return [[1, 0, 0, 0], [0, numpy.cos(roll), -numpy.sin(roll), 0], [0, numpy.sin(roll), numpy.cos(roll), 0],
[0, 0, 0, 1]]

1
submodules/intera_common Submodule

@ -0,0 +1 @@
Subproject commit a298fec02b8300e23841085a73c2039261c3d43c

1
submodules/intera_sdk Submodule

@ -0,0 +1 @@
Subproject commit 8dadd23d3ee696ae781a8a81c5b34f2efa23b2ee

1
submodules/sawyer_moveit Submodule

@ -0,0 +1 @@
Subproject commit 4fdb85bd2450be82bd05f3e0c742446dcb49f7cd

1
submodules/sawyer_robot Submodule

@ -0,0 +1 @@
Subproject commit 053334ec6e2cff2bafa6951b3f66c0ce6d43c0b3

1
submodules/sawyer_simulator Submodule

@ -0,0 +1 @@
Subproject commit cfb48a74f4013c866231f05a29e3757dc517a18f