diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt index 8c649ed65f675068f4c74bc4eccb4ce30f96ab51..461c7aeaf8fcd4f23aa7fef815a45f0807915613 100644 --- a/.catkin_tools/profiles/default/devel_collisions.txt +++ b/.catkin_tools/profiles/default/devel_collisions.txt @@ -1,4 +1,4 @@ /home/matteo/ws_panda/devel/./cmake.lock 42 -/home/matteo/reachability/devel/./cmake.lock 21925 +/home/matteo/reachability/devel/./cmake.lock 23953 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79 diff --git a/src/franka_description/robots/ceti_double.urdf.xacro b/src/franka_description/robots/ceti_double.urdf.xacro index 82c3dec04bcd299d594bc2d08cb5cfc24b03859a..50104dbbc69c278ad2bd91824190be7a469dcc3d 100644 --- a/src/franka_description/robots/ceti_double.urdf.xacro +++ b/src/franka_description/robots/ceti_double.urdf.xacro @@ -7,36 +7,45 @@ <xacro:include filename="panda_arm.xacro"/> <xacro:include filename="hand.xacro"/> + <xacro:arg name="scene" default='dummy.yaml'/> + <link name="world" /> - <xacro:property name="yaml_file" value="$(find mtc)/results/dummy.yaml" /> + <xacro:property name="yaml_file" value="$(find mtc)/results/dummy/-151902566.yaml" /> <xacro:property name="props" value="${xacro.load_yaml(yaml_file)}" /> <xacro:property name="x1" value="${props['objects'][8]['pos']['x']}" /> <xacro:property name="y1" value="${props['objects'][8]['pos']['y']}" /> <xacro:property name="z1" value="${props['objects'][8]['pos']['z']}" /> + <xacro:property name="sx1" value="${props['objects'][8]['size']['length']}" /> + <xacro:property name="sy1" value="${props['objects'][8]['size']['width']}" /> + <xacro:property name="sz1" value="${props['objects'][8]['size']['height']}" /> + + <xacro:property name="rpy1" value="${props['objects'][8]['rpy']['r']} ${props['objects'][8]['rpy']['p']} ${props['objects'][8]['rpy']['y']}" /> <xacro:property name="x2" value="${props['objects'][17]['pos']['x']}" /> <xacro:property name="y2" value="${props['objects'][17]['pos']['y']}" /> <xacro:property name="z2" value="${props['objects'][17]['pos']['z']}" /> <xacro:property name="rpy2" value="${props['objects'][17]['rpy']['r']} ${props['objects'][17]['rpy']['p']} ${props['objects'][17]['rpy']['y']}" /> + <xacro:property name="sx2" value="${props['objects'][17]['size']['length']}" /> + <xacro:property name="sy2" value="${props['objects'][17]['size']['width']}" /> + <xacro:property name="sz2" value="${props['objects'][17]['size']['height']}" /> - <!-- Dummy Link --> <joint name="base_joint1" type="fixed"> <parent link="world" /> <child link="base_1" /> - <origin xyz="${x1} ${y1} ${z1 * 0.5}" rpy="${rpy1}"/> + <origin xyz="${x1} ${y1} ${(z1* 0.5) + (sz1 * 0.25)}" rpy="${rpy1}"/> <axis xyz="0 0 1"/> </joint> <joint name="base_joint2" type="fixed"> <parent link="world" /> <child link="base_2" /> - <origin xyz="${x2} ${y2} ${z2 * 0.5}" rpy="${rpy2}"/> + <origin xyz="${x2} ${y2} ${(z2* 0.5) + (sz2 * 0.25)}" rpy="${rpy2}"/> <axis xyz="0 0 1"/> </joint> @@ -45,7 +54,7 @@ <visual> <origin xyz="0 0 0" rpy="0 0 0"/> <geometry> - <box size="0.8 0.8 ${z1}" /> + <box size="${sx1} ${sy1} ${z1 + (sz1 * 0.5)}" /> </geometry> <material name="White"> <color rgba="1.0 1.0 1.0 1.0"/> @@ -54,7 +63,7 @@ <collision> <origin xyz="0 0 0" rpy="0 0 0"/> <geometry> - <box size="0.8 0.8 ${z1}" /> + <box size="${sx1} ${sy1} ${z1 + (sz1 * 0.5)}" /> </geometry> </collision> </link> @@ -63,7 +72,7 @@ <visual> <origin xyz="0 0 0" rpy="0 0 0"/> <geometry> - <box size="0.8 0.8 ${z2}" /> + <box size="${sx2} ${sy2} ${z2 + (sz2 * 0.5)}" /> </geometry> <material name="White"> <color rgba="1.0 1.0 1.0 1.0"/> @@ -72,18 +81,19 @@ <collision> <origin xyz="0 0 0" rpy="0 0 0"/> <geometry> - <box size="0.8 0.8 ${z2}" /> + <box size="${sx2} ${sy2} ${z2 + (sz2 * 0.5)}" /> </geometry> </collision> </link> + <!-- right arm with gripper --> - <xacro:panda_arm arm_id="$(arg arm_id_1)" connected_to="base_1" xyz="-0.22 0 ${z1 * 0.5}" /> + <xacro:panda_arm arm_id="$(arg arm_id_1)" connected_to="base_1" xyz="-0.22 0 ${(z1* 0.5) + (sz1 * 0.25)}" /> <xacro:hand ns="$(arg arm_id_1)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_1)_link8" /> <!-- left arm with gripper --> - <xacro:panda_arm arm_id="$(arg arm_id_2)" connected_to="base_2" xyz="-0.22 0 ${z2 * 0.5}" /> + <xacro:panda_arm arm_id="$(arg arm_id_2)" connected_to="base_2" xyz="-0.22 0 ${(z2* 0.5) + (sz2 * 0.25)}" /> <xacro:hand ns="$(arg arm_id_2)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_2)_link8"/> </robot> diff --git a/src/mtc/CMakeLists.txt b/src/mtc/CMakeLists.txt index 91d4b6df4c08961ac1f05245f1e59740a68e2644..f052aacc56636e26791cd5399f53536172f276c3 100644 --- a/src/mtc/CMakeLists.txt +++ b/src/mtc/CMakeLists.txt @@ -38,14 +38,10 @@ find_package(catkin REQUIRED COMPONENTS trajectory_msgs pcl_ros yaml_to_mtc - ) # Catkin catkin_package( - LIBRARIES - moveit_grasps - moveit_grasps_filter CATKIN_DEPENDS geometry_msgs message_runtime @@ -102,42 +98,7 @@ src/impl/collision_helper.cpp -# Grasp Library -add_library(moveit_grasps - src/grasp_candidate.cpp - src/grasp_data.cpp - src/grasp_generator.cpp - src/grasp_scorer.cpp - src/two_finger_grasp_data.cpp - src/two_finger_grasp_generator.cpp - src/two_finger_grasp_scorer.cpp -) - -target_link_libraries(moveit_grasps - ${catkin_LIBRARIES} ${Boost_LIBRARIES} -) - -# Grasp Filter Library -add_library(moveit_grasps_filter - src/grasp_filter.cpp - src/two_finger_grasp_filter.cpp - src/grasp_planner.cpp -) -target_link_libraries(moveit_grasps_filter - moveit_grasps - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} -) - - -set_target_properties(moveit_grasps_filter PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") # for threading -set_target_properties(moveit_grasps_filter PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") - - - - add_executable(mtc2taskspace src/mtc2taskspace.cpp) -add_executable(moveit_grasps_grasp_pipeline src/grasp_pipeline.cpp) add_dependencies(base_routine ${base_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -145,10 +106,6 @@ add_dependencies(cell_routine ${cell_routine_EXPORTED_TARGETS} ${catkin_EXPORTED add_dependencies(mtc2taskspace ${mtc2taskspace_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(moveit_grasps_grasp_pipeline -moveit_grasps moveit_grasps_filter ${catkin_LIBRARIES} ${Boost_LIBRARIES} yaml-cpp -) - target_link_libraries(base_routine ${catkin_LIBRARIES} ${OCTOMAP_LIBRARIES} diff --git a/src/mtc/descriptions/dummy.yaml b/src/mtc/descriptions/dummy.yaml index 706923ede1df25cf7cd04ad27749581eac121f89..575dd6a88091d57391b747c6bd5e0c2cad90d672 100644 --- a/src/mtc/descriptions/dummy.yaml +++ b/src/mtc/descriptions/dummy.yaml @@ -1,6 +1,6 @@ task: groups: - robot_x: + panda_arm1: - -0.300000 -0.700000 0.890000 - -0.300000 -0.600000 0.890000 - -0.200000 -0.700000 0.890000 @@ -31,7 +31,7 @@ task: - -0.200000 0.700000 0.890000 - 0.100000 0.600000 0.890000 - 0.100000 0.700000 0.890000 - robot_y: + panda_arm2: - 0.100000 1.010000 0.890000 - 0.100000 1.110000 0.890000 - 0.100000 1.210000 0.890000 diff --git a/src/mtc/include/grasp_candidate.h b/src/mtc/include/grasp_candidate.h deleted file mode 100644 index e954038fbffbc54851d8e91b389edc0d72aa3ccb..0000000000000000000000000000000000000000 --- a/src/mtc/include/grasp_candidate.h +++ /dev/null @@ -1,142 +0,0 @@ -#ifndef MOVEIT_GRASPS__GRASP_CANDIDATE_ -#define MOVEIT_GRASPS__GRASP_CANDIDATE_ - -// ROS -#include <ros/ros.h> -#include <moveit_msgs/Grasp.h> - -// Grasping -#include "../include/grasp_data.h" - -// MoveIt -#include <moveit/robot_state/robot_state.h> -#include <moveit_msgs/Grasp.h> - -namespace moveit_grasps -{ -typedef std::vector<std::vector<moveit::core::RobotStatePtr> > GraspTrajectories; -enum GraspTrajectorySegments -{ - APPROACH = 0, - LIFT = 1, - RETREAT = 2 -}; - -struct GraspFilterCode -{ - enum - { - NOT_FILTERED = 0, - GRASP_FILTERED_BY_IK, // Ik solution at grasp failed - GRASP_FILTERED_BY_CUTTING_PLANE, // grasp pose is in an unreachable part of the environment (eg: behind a wall) - GRASP_FILTERED_BY_ORIENTATION, // grasp pose is not desireable - GRASP_FILTERED_BY_IK_CLOSED, // ik solution was fine with grasp opened, but failed with grasp closed - PREGRASP_FILTERED_BY_IK, // Ik solution before approach failed - GRASP_INVALID, // An error occured while processing the grasp - LAST // Used to track last value in the base class when inheriting - }; -}; - -/** - * \brief Contains collected data for each potential grasp after it has been verified / filtered - * This includes the pregrasp and grasp IK solution - */ -class GraspCandidate -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - GraspCandidate(const moveit_msgs::Grasp& grasp, const GraspDataPtr& grasp_data, const Eigen::Isometry3d& cuboid_pose); - - bool getPreGraspState(moveit::core::RobotStatePtr& robot_state); - - bool getGraspStateOpen(moveit::core::RobotStatePtr& robot_state); - - bool getGraspStateOpenEEOnly(moveit::core::RobotStatePtr& robot_state); - - bool getGraspStateClosed(moveit::core::RobotStatePtr& robot_state); - - bool getGraspStateClosedEEOnly(moveit::core::RobotStatePtr& robot_state); - - virtual bool isValid(); - - moveit_msgs::Grasp grasp_; - - /*# Contents of moveit_msgs::Grasp for reference - - # A name for this grasp - string id - - # The internal posture of the hand for the pre-grasp - # only positions are used - trajectory_msgs/JointTrajectory pre_grasp_posture - - # The internal posture of the hand for the grasp - # positions and efforts are used - trajectory_msgs/JointTrajectory grasp_posture - - # The position of the end-effector for the grasp. This is the pose of - # the "parent_link" of the end-effector, not actually the pose of any - # link *in* the end-effector. Typically this would be the pose of the - # most distal wrist link before the hand (end-effector) links began. - geometry_msgs/PoseStamped grasp_pose - - # The estimated probability of success for this grasp, or some other - # measure of how "good" it is. - float64 grasp_quality - - # The approach direction to take before picking an object - GripperTranslation pre_grasp_approach - { - # defines a translation for the gripper, used in pickup or place tasks - # for example for lifting an object off a table or approaching the table for placing - - # the direction of the translation - geometry_msgs/Vector3Stamped direction - - # the desired translation distance - float32 desired_distance - - # the min distance that must be considered feasible before the - # grasp is even attempted - float32 min_distance - } - - # The retreat direction to take after a grasp has been completed (object is attached) - GripperTranslation post_grasp_retreat - - # The retreat motion to perform when releasing the object; this information - # is not necessary for the grasp itself, but when releasing the object, - # the information will be necessary. The grasp used to perform a pickup - # is returned as part of the result, so this information is available for - # later use. - GripperTranslation post_place_retreat - - # the maximum contact force to use while grasping (<=0 to disable) - float32 max_contact_force - - # an optional list of obstacles that we have semantic information about - # and that can be touched/pushed/moved in the course of grasping - string[] allowed_touch_objects - - # The minimum opening that the fingers can have as it approaches an object - float64 min_finger_open_on_approach - */ - - const GraspDataPtr grasp_data_; - // TODO(davetcoleman): possibly remove - Eigen::Isometry3d cuboid_pose_; // pose of original object to grasp - - int grasp_filtered_code_; // All codes defined in enum - - std::vector<double> grasp_ik_solution_; - std::vector<double> pregrasp_ik_solution_; - - // Store pregrasp, grasp, lifted, and retreat trajectories - GraspTrajectories segmented_cartesian_traj_; -}; // class - -typedef std::shared_ptr<GraspCandidate> GraspCandidatePtr; - -} // namespace moveit_grasps - -#endif diff --git a/src/mtc/include/grasp_data.h b/src/mtc/include/grasp_data.h deleted file mode 100644 index 874f1da15b6771408ca9afdef8fb9dd9e454ff86..0000000000000000000000000000000000000000 --- a/src/mtc/include/grasp_data.h +++ /dev/null @@ -1,102 +0,0 @@ -#ifndef MOVEIT_GRASPS__GRASP_DATA_H_ -#define MOVEIT_GRASPS__GRASP_DATA_H_ - -// Ros -#include <ros/node_handle.h> - -// Msgs -#include <geometry_msgs/Pose.h> -#include <trajectory_msgs/JointTrajectory.h> - -// MoveIt -#include <moveit/macros/class_forward.h> -#include <moveit/robot_state/robot_state.h> -#include <moveit/robot_model/link_model.h> - -// moveit grasps -#include "suction_voxel_matrix.h" - -namespace moveit_grasps -{ -MOVEIT_CLASS_FORWARD(GraspData); - -// Map various arms to end effector grasp datas -typedef std::map<const robot_model::JointModelGroup*, moveit_grasps::GraspDataPtr> GraspDatas; - -struct GraspData -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /** - * \brief Loads grasp data from a yaml file (load from roslaunch) - * \param node handle - allows for namespacing - * \param end effector name - which side of a two handed robot to load data for. should correspond to SRDF EE names - */ - GraspData(const ros::NodeHandle& nh, const std::string& end_effector, - const moveit::core::RobotModelConstPtr& robot_model); - - /** - * \brief Helper function for constructor - * \return true on success - */ - virtual bool loadGraspData(const ros::NodeHandle& nh, const std::string& end_effector); - - /** - * \brief Alter a robot state so that the end effector corresponding to this grasp data is in pre-grasp state (OPEN) - * \param joint state of robot - * \return true on success - */ - bool setRobotStatePreGrasp(robot_state::RobotStatePtr& robot_state); - - /** - * \brief Alter a robot state so that the end effector corresponding to this grasp data is in grasp state (CLOSED) - * \param joint state of robot - * \return true on success - */ - bool setRobotStateGrasp(robot_state::RobotStatePtr& robot_state); - - /** - * \brief Alter a robot state so that the end effector corresponding to this grasp data is in a grasp posture - * \param joint state of robot - * \param posture - what state to set the end effector - * \return true on success - */ - bool setRobotState(robot_state::RobotStatePtr& robot_state, const trajectory_msgs::JointTrajectory& posture); - - /** - * \brief Debug data to console - */ - virtual void print(); - - // The (possibly fictional) center point of a grasp - std::string tcp_name_; - Eigen::Isometry3d tcp_to_eef_mount_; // Convert generic grasp pose to the parent arm's eef_mount frame of reference - - trajectory_msgs::JointTrajectory pre_grasp_posture_; // when the end effector is in "open" position - trajectory_msgs::JointTrajectory grasp_posture_; // when the end effector is in "close" position - std::string base_link_; // name of global frame with z pointing up - - const robot_model::JointModelGroup* ee_jmg_; // this end effector - const robot_model::JointModelGroup* arm_jmg_; // the arm that attaches to this end effector - const robot_model::RobotModelConstPtr robot_model_; - - // Duplicate end effector data copied from RobotModel - // the last link in the kinematic chain before the end effector, e.g. "/gripper_roll_link" class - const robot_model::LinkModel* parent_link_; - - int angle_resolution_; // generate grasps at increments of: angle_resolution * pi / 180 - - double grasp_resolution_; - double grasp_depth_resolution_; // generate grasps at this depth resolution along grasp_max_depth_ - double grasp_min_depth_; // minimum amount fingers must overlap object - double grasp_max_depth_; // Maximum distance from tip of end effector inwords that an object can be for a grasp - - // grasp approach and retreat parameters - double approach_distance_desired_; // this is in addition to the grasp_max_depth - double retreat_distance_desired_; // this is in addition to the grasp_max_depth - double lift_distance_desired_; - double grasp_padding_on_approach_; -}; - -} // namespace moveit_grasps - -#endif diff --git a/src/mtc/include/grasp_filter.h b/src/mtc/include/grasp_filter.h deleted file mode 100644 index a3ece959f2e7e36132244f190ee37ebe7bfa98b1..0000000000000000000000000000000000000000 --- a/src/mtc/include/grasp_filter.h +++ /dev/null @@ -1,391 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman <dave@picknik.ai> - Desc: Filters grasps based on kinematic feasibility and collision -*/ - -#ifndef MOVEIT_GRASPS__GRASP_FILTER_ -#define MOVEIT_GRASPS__GRASP_FILTER_ - -// ROS -#include <ros/ros.h> -#include <tf_conversions/tf_eigen.h> -#include <geometry_msgs/PoseArray.h> -#include <geometry_msgs/PoseStamped.h> - -// Grasping -#include "../include/grasp_generator.h" -#include "../include/grasp_candidate.h" - -// Rviz -#include <moveit_visual_tools/moveit_visual_tools.h> - -// MoveIt -#include <moveit/robot_state/robot_state.h> -#include <moveit_msgs/Grasp.h> -#include <moveit/kinematics_plugin_loader/kinematics_plugin_loader.h> -#include <moveit/planning_scene_monitor/planning_scene_monitor.h> - -// C++ -#include <boost/thread.hpp> -#include <cmath> - -namespace moveit_grasps -{ -enum GraspParallelPlane -{ - XY, - XZ, - YZ -}; - -/** - * \brief Contains information to filter grasps by a cutting plane - */ -struct CuttingPlane -{ - Eigen::Isometry3d pose_; - GraspParallelPlane plane_; - int direction_; - - CuttingPlane(Eigen::Isometry3d pose, GraspParallelPlane plane, int direction) - : pose_(pose), plane_(plane), direction_(direction) - { - } -}; -typedef std::shared_ptr<CuttingPlane> CuttingPlanePtr; - -/** - * \brief Contains information to filter grasps by orientation - */ -struct DesiredGraspOrientation -{ - Eigen::Isometry3d pose_; - double max_angle_offset_; - - DesiredGraspOrientation(Eigen::Isometry3d pose, double max_angle_offset) - : pose_(pose), max_angle_offset_(max_angle_offset) - { - } -}; -typedef std::shared_ptr<DesiredGraspOrientation> DesiredGraspOrientationPtr; - -/** - * \brief Struct for passing parameters to threads, for cleaner code - */ -struct IkThreadStruct -{ - IkThreadStruct(std::vector<GraspCandidatePtr>& grasp_candidates, // the input - const planning_scene::PlanningScenePtr& planning_scene, Eigen::Isometry3d& link_transform, - std::size_t grasp_id, kinematics::KinematicsBaseConstPtr kin_solver, - const robot_state::RobotStatePtr& robot_state, double timeout, bool filter_pregrasp, bool visual_debug, - std::size_t thread_id, const std::string& grasp_target_object_id) - : grasp_candidates_(grasp_candidates) - , planning_scene_(planning_scene::PlanningScene::clone(planning_scene)) - , link_transform_(link_transform) - , grasp_id(grasp_id) - , kin_solver_(kin_solver) - , robot_state_(std::make_shared<robot_state::RobotState>(*robot_state)) - , timeout_(timeout) - , filter_pregrasp_(filter_pregrasp) - , visual_debug_(visual_debug) - , thread_id_(thread_id) - , grasp_target_object_id_(grasp_target_object_id) - { - } - - std::vector<GraspCandidatePtr>& grasp_candidates_; - planning_scene::PlanningScenePtr planning_scene_; - Eigen::Isometry3d link_transform_; - std::size_t grasp_id; - kinematics::KinematicsBaseConstPtr kin_solver_; - robot_state::RobotStatePtr robot_state_; - double timeout_; - bool filter_pregrasp_; - bool visual_debug_; - std::size_t thread_id_; - // The name of the target grasp object to be used in the planning scene. - // NOTE: The ik filtering checks if the user has already added the grasp object - // to the planning scene. If it hasn't then GraspFilter will not modify the planning scene allowed collision matrix - std::string grasp_target_object_id_; - - // Used within processing function - geometry_msgs::PoseStamped ik_pose_; // Set from grasp candidate - std::vector<double> ik_seed_state_; -}; -typedef std::shared_ptr<IkThreadStruct> IkThreadStructPtr; - -class GraspFilter -{ -public: - // Constructor - GraspFilter(const robot_state::RobotStatePtr& robot_state, - const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools); - - /** - * \brief Return grasps that are kinematically feasible - * \param grasp_candidates - all possible grasps that this will test. this vector is returned modified - * \param arm_jmg - the arm to solve the IK problem on - * \param filter_pregrasp -whether to also check ik feasibility for the pregrasp position - * \param target_object_id - The name of the target grasp object in the planning scene if it exists - * \return true on successful filter - */ - virtual bool filterGrasps(std::vector<GraspCandidatePtr>& grasp_candidates, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const robot_model::JointModelGroup* arm_jmg, const moveit::core::RobotStatePtr& seed_state, - bool filter_pregrasp = false, const std::string& target_object_id = ""); - - virtual bool filterGrasps(std::vector<GraspCandidatePtr>& grasp_candidates, - const planning_scene::PlanningScenePtr& planning_scene, - const robot_model::JointModelGroup* arm_jmg, const moveit::core::RobotStatePtr& seed_state, - bool filter_pregrasp = false, const std::string& target_object_id = ""); - /** - * \brief Return grasps that are kinematically feasible - * \param grasp_candidates - all possible grasps that this will test. this vector is returned modified - * \param arm_jmg - the arm to solve the IK problem on - * \param seed_state - A robot state to be used for IK. Ideally this will be close to the desired goal configuration. - * \param filter_pregrasp - Whether to also check ik feasibility for the pregrasp position - * \param visualize - visualize IK filtering - * \param target_object_id - The name of the target grasp object in the planning scene if it exists - * \return true on successful filter - */ - virtual std::size_t filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const robot_model::JointModelGroup* arm_jmg, - const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, - bool visualize, const std::string& grasp_target_object_id = ""); - - virtual std::size_t filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates, - const planning_scene::PlanningScenePtr& planning_scene, - const robot_model::JointModelGroup* arm_jmg, - const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, - bool visualize, const std::string& grasp_target_object_id = ""); - - /** - * \brief Print grasp filtering statistics - */ - virtual void printFilterStatistics(const std::vector<GraspCandidatePtr>& grasp_candidates) const; - - /** - * \brief Method for checking part of the possible grasps list. MUST BE THREAD SAFE - */ - virtual bool processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct); - - /** - * \brief Used for sorting an array of CandidateGrasps - * \return true if A is less than B - */ - static bool compareGraspScores(const GraspCandidatePtr& grasp_a, const GraspCandidatePtr& grasp_b) - { - // Determine if A or B has higher quality - return (grasp_a->grasp_.grasp_quality > grasp_b->grasp_.grasp_quality); - } - - /** - * \brief Of an array of grasps, sort the valid ones from best score to worse score - * \return false if no grasps remain - */ - bool removeInvalidAndFilter(std::vector<GraspCandidatePtr>& grasp_candidates) const; - - /** - * \brief Add a cutting plane filter for a shelf bin - * \return true on success - */ - bool addCuttingPlanesForBin(const Eigen::Isometry3d& world_to_bin, const Eigen::Isometry3d& bin_to_product, - const double& bin_width, const double& bin_height); - - /* \brief Set the ACM entry to ignore collisions between the ee_link_names and the object in the planning_scene */ - void setACMFingerEntry(const std::string& object_name, bool allowed, const std::vector<std::string>& ee_link_names, - const planning_scene::PlanningScenePtr& planning_scene); - -protected: - /** - * \brief Filter grasps by cutting plane - * \param grasp_candidates - all possible grasps that this will test. this vector is returned modified - * \param filter_pose - pose of filter that will define cutting plane - * \param plane - the cutting plane (XY, XZ, or YZ) - * \param direction - which side of this plane to cut (+/- 1) - * \return true if grasp is filtered by operation - */ - bool filterGraspByPlane(GraspCandidatePtr& grasp_candidate, const Eigen::Isometry3d& filter_pose, - GraspParallelPlane plane, int direction) const; - - /** - * \brief Filter grasps by desired orientation. Think of reaching into a small opening, you can only rotate your hand - * a tiny amount and still grasp an object. If there's empty space behind an object, grasps behind the object aren't - * removed by the cutting plane operations. We know we'll never get to them because they deviate too much from the - * desired grasping pose... straight in. - * \param grasp_candidates - a grasp candidate that this will test. - * \param desired_pose - the desired grasp pose ( using standard grasping orientation ) - * \param max_angular_offset - maximum angle allowed between the grasp pose and the desired pose - * \return true if grasp is filtered by operation - */ - bool filterGraspByOrientation(GraspCandidatePtr& grasp_candidate, const Eigen::Isometry3d& desired_pose, - double max_angular_offset) const; - - /** - * \brief Filter grasps by feasablity of the grasp pose. - * \param grasp_candidates - a grasp candidate that this will test - * \param grasp_ik_solution - the IK solution if one exists - * \param ik_thread_struct - a struct containing the planning_scene - * \return true if grasp is filtered by operation - */ - bool filterGraspByGraspIK(const GraspCandidatePtr& grasp_candidate, std::vector<double>& grasp_ik_solution, - const IkThreadStructPtr& ik_thread_struct) const; - - /** - * \brief Filter grasps by feasablity of the pre grasp pose. - * \param grasp_candidates - a grasp candidate that this will test - * \param grasp_ik_solution - the IK solution if one exists - * \param ik_thread_struct - a struct containing the planning_scene - * \return true if grasp is filtered by operation - */ - bool filterGraspByPreGraspIK(const GraspCandidatePtr& grasp_candidate, std::vector<double>& pregrasp_ik_solution, - const IkThreadStructPtr& ik_thread_struct) const; - - /** - * \brief Helper for the thread function to find IK solutions - * \return true on success - */ - bool findIKSolution(std::vector<double>& ik_solution, const IkThreadStructPtr& ik_thread_struct, - const GraspCandidatePtr& grasp_candidate, - const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const; - - /** - * \brief add a cutting plane - * \param pose - pose describing the cutting plane - * \param plane - which plane to use as the cutting plane - * \param direction - on which side of the plane the grasps will be removed - */ - void addCuttingPlane(const Eigen::Isometry3d& pose, GraspParallelPlane plane, int direction); - - /** - * \brief Show all cutting planes that are currently enables - * \return true on success - */ - bool visualizeCuttingPlanes(); - - /** - * \brief clear all cutting planes - */ - void clearCuttingPlanes(); - - /** - * \brief add a desired grasp orientation - * \param pose - the desired grasping pose - * \param max_angle_offset - maximum amount a generated grasp can deviate from the desired pose - */ - void addDesiredGraspOrientation(const Eigen::Isometry3d& pose, double max_angle_offset); - - /** - * \brief clear all desired orientations - */ - void clearDesiredGraspOrientations(); - - /** - * \brief Show grasps after being filtered - * \return true on success - */ - bool visualizeGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates, - const moveit::core::JointModelGroup* arm_jmg); - - /** - * \brief Show IK solutions of entire arm - * \return true on success - */ - bool visualizeIKSolutions(const std::vector<GraspCandidatePtr>& grasp_candidates, - const moveit::core::JointModelGroup* arm_jmg, double animation_speed); - - /** - * \brief Show solutions of entire arm - * \return true on success - */ - bool visualizeCandidateGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates); - - /** - * \brief Show an entire planning scene - */ - void publishPlanningScene(const planning_scene::PlanningScenePtr& ps) const; - -protected: - // logging name - const std::string name_; - - // Allow a writeable robot state - robot_state::RobotStatePtr robot_state_; - - // Keep a robot state for every thread - std::vector<robot_state::RobotStatePtr> robot_states_; - - // Threaded kinematic solvers - std::map<std::string, std::vector<kinematics::KinematicsBaseConstPtr> > kin_solvers_; - - // Class for publishing stuff to rviz - moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; - // for rviz visualization of the planning scene - ros::Publisher planning_scene_publisher_; - - // Number of degrees of freedom for the IK solver to find - std::size_t num_variables_; - - // Time to allow IK solver to run - double solver_timeout_; - - // Visualization levels - bool collision_verbose_; - bool statistics_verbose_; - double collision_verbose_speed_; - bool show_filtered_grasps_; - bool show_filtered_arm_solutions_; - bool show_cutting_planes_; - double show_filtered_arm_solutions_speed_; - double show_filtered_arm_solutions_pregrasp_speed_; - bool show_grasp_filter_collision_if_failed_; - - // Shared node handle - ros::NodeHandle nh_; - - // Cutting planes and orientation filter - std::vector<CuttingPlanePtr> cutting_planes_; - std::vector<DesiredGraspOrientationPtr> desired_grasp_orientations_; - -}; // end of class - -typedef std::shared_ptr<GraspFilter> GraspFilterPtr; -typedef std::shared_ptr<const GraspFilter> GraspFilterConstPtr; - -} // namespace moveit_grasps - -#endif diff --git a/src/mtc/include/grasp_generator.h b/src/mtc/include/grasp_generator.h deleted file mode 100644 index 4d8a3f55b7d4af2e6fbcc4610b98bd8d14d3bfee..0000000000000000000000000000000000000000 --- a/src/mtc/include/grasp_generator.h +++ /dev/null @@ -1,248 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy - Desc: Generates geometric grasps for cuboids and blocks, not using physics or contact wrenches -*/ - -#ifndef MOVEIT_GRASPS__GRASP_GENERATOR_H_ -#define MOVEIT_GRASPS__GRASP_GENERATOR_H_ - -// ROS -#include <ros/ros.h> - -// TF -#include <tf_conversions/tf_eigen.h> - -// Msgs -#include <geometry_msgs/PoseArray.h> - -// MoveIt -#include <moveit_msgs/Grasp.h> - -// geometric_shapes -#include <geometric_shapes/shape_operations.h> -#include <geometric_shapes/bodies.h> - -// Eigen -#include <Eigen/Core> -#include <Eigen/Geometry> -#include <Eigen/Eigenvalues> -#include <eigen_conversions/eigen_msg.h> - -// Visualization -#include <moveit_visual_tools/moveit_visual_tools.h> - -// moveit_grasps -#include "grasp_candidate.h" -#include "grasp_scorer.h" - -// bounding_box -//#include <bounding_box/bounding_box.h> - -// C++ -#include <cstdlib> -#include <string> -#include <cmath> -#include <limits> - -#include "grasp_data.h" - -namespace moveit_grasps -{ -static const double RAD2DEG = 57.2957795; -static const double MIN_GRASP_DISTANCE = 0.001; // m between grasps - -// Grasp axis orientation -enum grasp_axis_t -{ - X_AXIS, - Y_AXIS, - Z_AXIS -}; - -class GraspGenerator -{ -public: - // Eigen requires 128-bit alignment for the Eigen::Vector2d's array (of 2 doubles). - // With GCC, this is done with a attribute ((aligned(16))). - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** - * \brief Constructor - */ - GraspGenerator(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, bool verbose = false); - - /** - * \brief Create possible grasp positions around a cuboid - * \param cuboid_pose - centroid of object to grasp in world frame - * \param depth length of cuboid along local x-axis - * \param width length of cuboid along local y-axis - * \param height length of cuboid along local z-axis - * \param grasp_data data describing end effector - * \param grasp_candidate_config parameter for selectively enabling and disabling different grasp types - * \param grasp_candidates possible grasps generated - * \return true if successful - */ - virtual bool generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height, - const GraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates) = 0; - - /** - * \brief Get the grasp direction vector relative to the world frame - * \param grasp - * \param name of parent link - * \return the approach direction - */ - static Eigen::Vector3d getPreGraspDirection(const moveit_msgs::Grasp& grasp, const std::string& ee_parent_link); - // static Eigen::Vector3d getPostGraspDirection(const moveit_msgs::Grasp &grasp, const std::string &ee_parent_link); - - /** - * \brief Using an input grasp description, get the pregrasp pose. The pregrasp pose is the grasp pose translated - * backwards in the grasp frame away from the object being grasped. - * \param grasp - * \param name of parent link - * \return pregrasp pose - */ - static geometry_msgs::PoseStamped getPreGraspPose(const GraspCandidatePtr& grasp_candidate, - const std::string& ee_parent_link); - /** - * \brief Compute the pre-grasp, grasp, lift and retreat poses for a grasp candidate - * \param grasp_candidate - the grasp candidate - * \param grasp_waypoints - a reference to a vector that will be populated with the pre-grasp, grasp, lift and retreat - * poses in that order. - */ - static void getGraspWaypoints(const GraspCandidatePtr& grasp_candidate, EigenSTL::vector_Isometry3d& grasp_waypoints); - - /** - * \brief Helper to convert a robot-specific grasp to an arrow pointed in the right direction - * \param grasp - the grasp to show - * \param arm - the planning group of the arm we want to display - * \return true on success - */ - void publishGraspArrow(const geometry_msgs::Pose& grasp, const GraspDataPtr& grasp_data, - const rviz_visual_tools::colors& color, double approach_length = 0.1); - - /** - * \brief Getter for Verbose - */ - bool getVerbose() - { - return verbose_; - } - - /** - * \brief Getter for ideal grasp pose - */ - Eigen::Isometry3d getIdealTCPGraspPose() - { - return ideal_grasp_pose_; - } - - /** - * \brief Setter for ideal grasp pose for scoring - */ - void setIdealTCPGraspPose(Eigen::Isometry3d ideal_pose) - { - ideal_grasp_pose_ = ideal_pose; - } - - /** - * \brief Setter for the roll pitch yall ideal grasp pose for scoring - */ - void setIdealTCPGraspPoseRPY(const std::vector<double>& ideal_grasp_orientation_rpy); - - /** - * \brief Setter for grasp score weights - */ - void setGraspScoreWeights(const GraspScoreWeightsPtr& grasp_score_weights) - { - grasp_score_weights_ = std::make_shared<GraspScoreWeights>(*grasp_score_weights); - } - - /** - * \brief Setter for grasp score weights - */ - const GraspScoreWeightsPtr& getGraspScoreWeights() - { - return grasp_score_weights_; - } - - /** - * \brief Setter for Verbose - */ - void setVerbose(bool verbose) - { - verbose_ = verbose; - } - - /** - * \brief Visualize animated grasps - * \return true on success - */ - bool visualizeAnimatedGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates, - const moveit::core::JointModelGroup* ee_jmg, double animation_speed); - - // Ideal grasp pose for scoring purposes - Eigen::Isometry3d ideal_grasp_pose_; - -protected: - // class for publishing stuff to rviz - moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; - - // Display more output both in console - bool verbose_; - - // Visual debug settings - bool show_prefiltered_grasps_; - double show_prefiltered_grasps_speed_; - - // Shared node handle - ros::NodeHandle nh_; - - // Transform from frame of box to global frame - Eigen::Isometry3d object_global_transform_; - - double min_grasp_distance_, max_grasp_distance_; - Eigen::Vector3d min_translations_, max_translations_; - - GraspScoreWeightsPtr grasp_score_weights_; - -}; // end of class - -typedef std::shared_ptr<GraspGenerator> GraspGeneratorPtr; -typedef std::shared_ptr<const GraspGenerator> GraspGeneratorConstPtr; - -} // namespace moveit_grasps - -#endif diff --git a/src/mtc/include/grasp_planner copy.h b/src/mtc/include/grasp_planner copy.h deleted file mode 100644 index a5271665b775d7b2a073298ef58f5eb2754db64e..0000000000000000000000000000000000000000 --- a/src/mtc/include/grasp_planner copy.h +++ /dev/null @@ -1,163 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman <dave@picknik.ai> - Desc: Find the approach, lift, and retreat path for a candidate grasp (if a valid one exists) -*/ - -#ifndef MOVEIT_GRASPS__GRASP_PLANNER_ -#define MOVEIT_GRASPS__GRASP_PLANNER_ - -// ROS -#include <ros/ros.h> - -// moveit_grasps -#include "../include/grasp_candidate.h" -#include "../include/grasp_generator.h" - -// moveit -#include <moveit_visual_tools/moveit_visual_tools.h> - -namespace moveit_grasps -{ -// Allow an interrupt to be called that waits for user input, useful for debugging -typedef boost::function<void(std::string message)> WaitForNextStepCallback; - -class GraspPlanner -{ -public: - /** - * \brief Constructor - */ - GraspPlanner(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools); - - /** - * \brief Plan entire cartesian manipulation sequence - * \param grasp_candidates - GraspCandidates for which we will compute apprach, lift and retreat paths - * \param robot_state - robot_state to be used for computeCartesianPath - * \param planning_scene_monitor - Current state of the world - * \param grasp_data - robot gripper configuration - * \param grasp_object_id - The name of the target grasp object to be used in the planning scene. - * \return true on success - */ - bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates, - const robot_state::RobotStatePtr& robot_state, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::string& grasp_object_id = ""); - - bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates, - const robot_state::RobotStatePtr& robot_state, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const std::string& grasp_object_id = ""); - - /** - * \brief Plan entire cartesian manipulation sequence - * \param input - description - * \return true on success - */ - bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - bool verbose_cartesian_filtering, const std::string& grasp_object_id = ""); - - bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state, - const planning_scene::PlanningSceneConstPtr& planning_scene, - bool verbose_cartesian_filtering, const std::string& grasp_object_id = ""); - - /** - * \brief Compute a cartesian path along waypoints - * \return true on success - */ - bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const moveit::core::RobotStatePtr& start_state, - const EigenSTL::vector_Isometry3d& waypoints, - const std::string& grasp_object_id = ""); - - bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit::core::RobotStatePtr& start_state, - const EigenSTL::vector_Isometry3d& waypoints, - const std::string& grasp_object_id = ""); - - /** - * \brief Wait for user input to proceeed - * \param message - text to display to user when waiting - */ - void waitForNextStep(const std::string& message); - - /** - * \brief Allow an interrupt to be called that waits for user input, useful for debugging - * \param message - text to display to user when waiting - */ - // TODO(davetcoleman): Ideally all the classes in moveit_grasps would leverage this approach - void setWaitForNextStepCallback(WaitForNextStepCallback callback); - - /** - * \brief Load parameter settings of the server under ~/debug_level - * \param parent_name - only used for debugging, allows one to see what paremeters are loaded in what namespace - * \param setting_namespace - where on this node's namespace to load settings - * e.g. /this_name/setting_namespace/some_parameter - * \return true on success - */ - bool loadEnabledSettings(); - - /** - * \brief Check if a setting is enabled - * \param setting_name - name of key on the parameter server as loaded in the 'setting_namespace' - * \return true if setting is enabled - */ - bool isEnabled(const std::string& setting_name); - -private: - // A shared node handle - ros::NodeHandle nh_; - - // Class for publishing stuff to rviz - moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; - - WaitForNextStepCallback wait_for_next_step_callback_; - - // Visualization settings - bool enabled_settings_loaded_ = false; - std::map<std::string, bool> enabled_setting_; - -}; // end class - -// Create smart pointers for this class -typedef std::shared_ptr<GraspPlanner> GraspPlannerPtr; -typedef std::shared_ptr<const GraspPlanner> GraspPlannerConstPtr; - -} // namespace moveit_grasps - -#endif diff --git a/src/mtc/include/grasp_planner.h b/src/mtc/include/grasp_planner.h deleted file mode 100644 index 9b5d922f63884718faa38b90076d0b9df4bad31a..0000000000000000000000000000000000000000 --- a/src/mtc/include/grasp_planner.h +++ /dev/null @@ -1,163 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman <dave@picknik.ai> - Desc: Find the approach, lift, and retreat path for a candidate grasp (if a valid one exists) -*/ - -#ifndef MOVEIT_GRASPS__GRASP_PLANNER_ -#define MOVEIT_GRASPS__GRASP_PLANNER_ - -// ROS -#include <ros/ros.h> - -// moveit_grasps -#include "grasp_candidate.h" -#include "grasp_generator.h" - -// moveit -#include <moveit_visual_tools/moveit_visual_tools.h> - -namespace moveit_grasps -{ -// Allow an interrupt to be called that waits for user input, useful for debugging -typedef boost::function<void(std::string message)> WaitForNextStepCallback; - -class GraspPlanner -{ -public: - /** - * \brief Constructor - */ - GraspPlanner(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools); - - /** - * \brief Plan entire cartesian manipulation sequence - * \param grasp_candidates - GraspCandidates for which we will compute apprach, lift and retreat paths - * \param robot_state - robot_state to be used for computeCartesianPath - * \param planning_scene_monitor - Current state of the world - * \param grasp_data - robot gripper configuration - * \param grasp_object_id - The name of the target grasp object to be used in the planning scene. - * \return true on success - */ - bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates, - const robot_state::RobotStatePtr& robot_state, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::string& grasp_object_id = ""); - - bool planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates, - const robot_state::RobotStatePtr& robot_state, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const std::string& grasp_object_id = ""); - - /** - * \brief Plan entire cartesian manipulation sequence - * \param input - description - * \return true on success - */ - bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - bool verbose_cartesian_filtering, const std::string& grasp_object_id = ""); - - bool planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, const robot_state::RobotStatePtr& robot_state, - const planning_scene::PlanningSceneConstPtr& planning_scene, - bool verbose_cartesian_filtering, const std::string& grasp_object_id = ""); - - /** - * \brief Compute a cartesian path along waypoints - * \return true on success - */ - bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const moveit::core::RobotStatePtr& start_state, - const EigenSTL::vector_Isometry3d& waypoints, - const std::string& grasp_object_id = ""); - - bool computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit::core::RobotStatePtr& start_state, - const EigenSTL::vector_Isometry3d& waypoints, - const std::string& grasp_object_id = ""); - - /** - * \brief Wait for user input to proceeed - * \param message - text to display to user when waiting - */ - void waitForNextStep(const std::string& message); - - /** - * \brief Allow an interrupt to be called that waits for user input, useful for debugging - * \param message - text to display to user when waiting - */ - // TODO(davetcoleman): Ideally all the classes in moveit_grasps would leverage this approach - void setWaitForNextStepCallback(WaitForNextStepCallback callback); - - /** - * \brief Load parameter settings of the server under ~/debug_level - * \param parent_name - only used for debugging, allows one to see what paremeters are loaded in what namespace - * \param setting_namespace - where on this node's namespace to load settings - * e.g. /this_name/setting_namespace/some_parameter - * \return true on success - */ - bool loadEnabledSettings(); - - /** - * \brief Check if a setting is enabled - * \param setting_name - name of key on the parameter server as loaded in the 'setting_namespace' - * \return true if setting is enabled - */ - bool isEnabled(const std::string& setting_name); - -private: - // A shared node handle - ros::NodeHandle nh_; - - // Class for publishing stuff to rviz - moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; - - WaitForNextStepCallback wait_for_next_step_callback_; - - // Visualization settings - bool enabled_settings_loaded_ = false; - std::map<std::string, bool> enabled_setting_; - -}; // end class - -// Create smart pointers for this class -typedef std::shared_ptr<GraspPlanner> GraspPlannerPtr; -typedef std::shared_ptr<const GraspPlanner> GraspPlannerConstPtr; - -} // namespace moveit_grasps - -#endif diff --git a/src/mtc/include/grasp_scorer.h b/src/mtc/include/grasp_scorer.h deleted file mode 100644 index c1b5f671ecdaa1c0799787ef439549911776db23..0000000000000000000000000000000000000000 --- a/src/mtc/include/grasp_scorer.h +++ /dev/null @@ -1,126 +0,0 @@ -/********************************************************************* - * Software License Agreement ("Modified BSD License") - * - * Copyright (c) 2014, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ -/** - * Authors : Andy McEvoy - * Desc : Functions for scoring generated grasps - */ - -#ifndef MOVEIT_GRASPS__GRASP_SCORER_ -#define MOVEIT_GRASPS__GRASP_SCORER_ - -#include <cmath> - -#include <ros/ros.h> - -#include "../include/grasp_data.h" -#include <moveit_visual_tools/moveit_visual_tools.h> - -#include <Eigen/Core> -#include <Eigen/Geometry> - -namespace moveit_grasps -{ -struct GraspScoreWeights -{ - GraspScoreWeights() - : orientation_x_score_weight_(1.0) - , orientation_y_score_weight_(1.0) - , orientation_z_score_weight_(1.0) - , translation_x_score_weight_(1.0) - , translation_y_score_weight_(1.0) - , translation_z_score_weight_(1.0) - { - } - - /* \brief Compute the weighted score given the orientation and translation scores */ - double computeScore(const Eigen::Vector3d& orientation_scores, const Eigen::Vector3d& translation_scores, - bool verbose = false) const; - - /* \brief returns the sum of the grasp score weights*/ - virtual double getWeightTotal() const; - - double orientation_x_score_weight_; - double orientation_y_score_weight_; - double orientation_z_score_weight_; - double translation_x_score_weight_; - double translation_y_score_weight_; - double translation_z_score_weight_; -}; -// Create smart pointers for this struct -typedef std::shared_ptr<GraspScoreWeights> GraspScoreWeightsPtr; - -class GraspScorer -{ -public: - /** - * \brief Scores each axis of the grasp based on its angle to the desired pose axis. - * \param grasp_pose_tcp - the pose of the end effector - * \param ideal_pose - the ideal grasp pose (ex: straight into the bin) - * \return the unweighted scores: - * 1.0 -> 0 degrees between grasp axis and desired axis, - * 0.0 -> 180 degrees - */ - static Eigen::Vector3d scoreRotationsFromDesired(const Eigen::Isometry3d& grasp_pose_tcp, - const Eigen::Isometry3d& ideal_pose); - - /** - * \brief Score the grasp based on the translation values of the grasp pose - * \param grasp_pose_tcp - the pose of the end effector (not the eef mount) - * \param min_translations - the minimum translation values for all grasp poses - * \param max_translations - the maximum translation values for all grasp poses - * \return the unweighted scores: - * 0.0 -> pose is at the minimum translation in that axis - * 1.0 -> pose is at the maximum translation in that axis - */ - static Eigen::Vector3d scoreGraspTranslation(const Eigen::Isometry3d& grasp_pose_tcp, - const Eigen::Vector3d& min_translations, - const Eigen::Vector3d& max_translations); - - /** - * \brief Score the grasp based on the translation values of the grasp pose - * \param grasp_pose_tcp - the pose of the end effector (not the eef mount) - * \param ideal_pose - the ideal pose location - * \param object_pose - the pose of the object being grasped - * \param object_size - the size of the object represented as a vector [x,y,z] - * \param visual_tools - set to a moveit_visual_tools pointer to enable visual debugging - * \return the unweighted scores: - * 0.0 -> pose is at the ideal translation in that axis - */ - static Eigen::Vector3d scoreGraspTranslation(const Eigen::Isometry3d& grasp_pose_tcp, - const Eigen::Isometry3d& ideal_pose); -}; - -} // namespace moveit_grasps - -#endif diff --git a/src/mtc/include/impl/moveit_mediator.h b/src/mtc/include/impl/moveit_mediator.h index 9d75774ec6f564df90f106dd71fa3b10fbdf62d6..8b73a8e11a41cb56a3d514fc058c8442e0d62fba 100644 --- a/src/mtc/include/impl/moveit_mediator.h +++ b/src/mtc/include/impl/moveit_mediator.h @@ -15,20 +15,49 @@ #include "impl/robot.h" #include "impl/moveit_robot.h" +#include "impl/wing_moveit_decorator.h" +#include "impl/collision_helper.h" +#include <moveit/task_constructor/task.h> +#include <moveit/task_constructor/stages/compute_ik.h> +#include <moveit/task_constructor/stages/connect.h> +#include <moveit/task_constructor/stages/current_state.h> +#include <moveit/task_constructor/stages/generate_grasp_pose.h> +#include <moveit/task_constructor/stages/generate_pose.h> +#include <moveit/task_constructor/stages/generate_place_pose.h> +#include <moveit/task_constructor/stages/modify_planning_scene.h> +#include <moveit/task_constructor/stages/move_relative.h> +#include <moveit/task_constructor/stages/move_to.h> +#include <moveit/task_constructor/stages/predicate_filter.h> +#include <moveit/task_constructor/solvers/cartesian_path.h> +#include <moveit/task_constructor/solvers/pipeline_planner.h> +#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h> +#include <eigen_conversions/eigen_msg.h> + class Moveit_mediator : public Abstract_mediator{ protected: ros::NodeHandle nh_; - ros::Publisher collision_object_publisher_; - ros::Publisher planning_scene_diff_publisher_; robot_model_loader::RobotModelLoader* robot_model_loader_; robot_model::RobotModelPtr kinematic_model_; planning_scene::PlanningScene* ps_; XmlRpc::XmlRpcValue properties_; + std::shared_ptr<moveit::task_constructor::solvers::PipelinePlanner> sampling_planner_; + std::shared_ptr<moveit::task_constructor::solvers::CartesianPath> cartesian_planner_; + std::unique_ptr<moveit::planning_interface::PlanningSceneInterface> psi_; + std::map<std::string, std::vector<moveit::task_constructor::Task>> task_map_; + + + public: - Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, ros::NodeHandle nh) : Abstract_mediator(objects, pub), nh_(nh){ - collision_object_publisher_ = nh.advertise<moveit_msgs::CollisionObject>("collision_object", 1); - planning_scene_diff_publisher_ = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1); + Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, ros::NodeHandle nh) : Abstract_mediator(objects, pub), nh_(nh), sampling_planner_(std::make_unique<moveit::task_constructor::solvers::PipelinePlanner>()), cartesian_planner_(std::make_unique<moveit::task_constructor::solvers::CartesianPath>()), psi_(std::make_unique<moveit::planning_interface::PlanningSceneInterface>()){ + // planner + sampling_planner_->setProperty("goal_joint_tolerance", 1e-5); + + // cartesian + cartesian_planner_->setMaxVelocityScaling(1.0); + cartesian_planner_->setMaxAccelerationScaling(1.0); + cartesian_planner_->setStepSize(.01); + }; void setup_task(); @@ -39,9 +68,13 @@ class Moveit_mediator : public Abstract_mediator{ void set_wings(std::vector<std::vector<wing_BP>>& wbp) override; - void publish_tables(moveit::planning_interface::PlanningSceneInterface& psi); + void publish_tables(); void load_robot_description(); void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); + void create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target); + inline std::map<std::string, std::vector<moveit::task_constructor::Task>>& task_map(){return task_map_;}; + + }; #endif \ No newline at end of file diff --git a/src/mtc/include/impl/moveit_robot.h b/src/mtc/include/impl/moveit_robot.h index 60e789438cf403620374056ec374f7263179fc5a..ae1c707684a95ac47bc6da6152f5f60dbbe826a4 100644 --- a/src/mtc/include/impl/moveit_robot.h +++ b/src/mtc/include/impl/moveit_robot.h @@ -17,16 +17,14 @@ class Moveit_robot : public Robot{ Moveit_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Robot(name, tf, size){ mgi_ = new moveit::planning_interface::MoveGroupInterface(name); std::stringstream hand_n, ik_frame_n, name_n; - name_n << name; hand_n << "hand_" << name.back(); ik_frame_n << "panda_" << name.back() << "_link8"; - map_.insert(std::make_pair<std::string, std::string>("group", name_n.str())); - map_.insert(std::make_pair<std::string, std::string>("eef", hand_n.str())); - map_.insert(std::make_pair<std::string, std::string>("hand_grasping_frame", ik_frame_n.str())); - map_.insert(std::make_pair<std::string, std::string>("ik_frame", ik_frame_n.str())); - map_.insert(std::make_pair<std::string, std::string>("hand", hand_n.str())); + map_.insert(std::make_pair<std::string, std::string>("eef_name", hand_n.str())); + map_.insert(std::make_pair<std::string, std::string>("hand_frame", ik_frame_n.str())); + map_.insert(std::make_pair<std::string, std::string>("hand_group_name", hand_n.str())); + } inline moveit::planning_interface::MoveGroupInterface* mgi() {return mgi_;} diff --git a/src/mtc/include/state_validity_callback.h b/src/mtc/include/state_validity_callback.h deleted file mode 100644 index 575beb89ad141215474389e0b5fc6dd78f65b9d7..0000000000000000000000000000000000000000 --- a/src/mtc/include/state_validity_callback.h +++ /dev/null @@ -1,87 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman <dave@picknik.ai> - Desc: Callback for checking if a state is in collision -*/ - -#ifndef MOVEIT_GRASPS__STATE_VALIDITY_CALLBACK -#define MOVEIT_GRASPS__STATE_VALIDITY_CALLBACK - -// Rviz -#include <moveit_visual_tools/moveit_visual_tools.h> - -// MoveIt -#include <moveit/planning_scene_monitor/planning_scene_monitor.h> -#include <moveit/robot_state/robot_state.h> - -namespace -{ -bool isGraspStateValid(const planning_scene::PlanningScene* planning_scene, bool visual_debug, double verbose_speed, - const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, - robot_state::RobotState* robot_state, const robot_state::JointModelGroup* group, - const double* ik_solution) -{ - robot_state->setJointGroupPositions(group, ik_solution); - robot_state->update(); - if (!robot_state->satisfiesBounds(group)) - { - ROS_DEBUG_STREAM_NAMED("is_grasp_state_valid", "Ik solution invalid"); - return false; - } - - if (!planning_scene) - { - ROS_ERROR_STREAM_NAMED("is_grasp_state_valid", "No planning scene provided"); - return false; - } - - if (!planning_scene->isStateColliding(*robot_state, group->getName())) - return true; // not in collision - - // Display more info about the collision - if (visual_debug && visual_tools) - { - visual_tools->publishRobotState(*robot_state, rviz_visual_tools::RED); - planning_scene->isStateColliding(*robot_state, group->getName(), true); - visual_tools->publishContactPoints(*robot_state, planning_scene); - visual_tools->trigger(); - ros::Duration(verbose_speed).sleep(); - } - return false; -} - -} // namespace - -#endif diff --git a/src/mtc/include/suction_voxel_matrix.h b/src/mtc/include/suction_voxel_matrix.h deleted file mode 100644 index 9bb35a6a59657097cdb1bef148e70ff80e378fb7..0000000000000000000000000000000000000000 --- a/src/mtc/include/suction_voxel_matrix.h +++ /dev/null @@ -1,188 +0,0 @@ -/* - * Software License Agreement (Modified BSD License) - * - * Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Univ of CO, Boulder, PAL Robotics, S.L. - * nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific - * prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/* Authors: Mike Lautman - Description: Data class used by grasp data. -*/ - -#ifndef MOVEIT_GRASPS__SUCTION_VOXEL_MATRIX_H_ -#define MOVEIT_GRASPS__SUCTION_VOXEL_MATRIX_H_ - -// ROS -#include <ros/ros.h> - -// Eigen -#include <Eigen/Core> -#include <Eigen/Geometry> - -namespace moveit_grasps -{ -struct SuctionVoxel -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /* \brief - @param center_point - position of voxel center point in tcp frame - @param x_width - width of voxel along x dim in tcp frame - @param y_width - width of voxel along y dim in tcp frame - */ - SuctionVoxel(const Eigen::Vector3d& center_point, double x_width, double y_width) - : center_point_(center_point), x_width_(x_width), y_width_(y_width) - { - top_left_ = center_point + Eigen::Vector3d(-x_width / 2.0, y_width / 2.0, 0); - top_right_ = center_point + Eigen::Vector3d(x_width / 2.0, y_width / 2.0, 0); - bottom_left_ = center_point + Eigen::Vector3d(-x_width / 2.0, -y_width / 2.0, 0); - bottom_right_ = center_point + Eigen::Vector3d(x_width / 2.0, -y_width / 2.0, 0); - } - - // Voxel center point in tcp frame - Eigen::Vector3d center_point_; - double x_width_; - double y_width_; - // Voxel corners in tcp frame - Eigen::Vector3d top_left_; - Eigen::Vector3d top_right_; - Eigen::Vector3d bottom_left_; - Eigen::Vector3d bottom_right_; -}; - -class SuctionVoxelMatrix -{ -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - SuctionVoxelMatrix(double suction_rows_count, double suction_cols_count, double total_suction_range_y, - double total_suction_range_x) - : suction_rows_count_(suction_rows_count) - , suction_cols_count_(suction_cols_count) - , active_suction_range_x_(total_suction_range_x) - , active_suction_range_y_(total_suction_range_y) - { - voxel_x_width_ = active_suction_range_x_ / suction_cols_count_; - voxel_y_width_ = active_suction_range_y_ / suction_rows_count_; - suction_voxels_.resize(suction_rows_count_); - // We store the voxels starting bottom left and moving right then up - for (std::size_t voxel_y = 0; voxel_y < suction_rows_count_; ++voxel_y) - { - suction_voxels_[voxel_y].resize(suction_cols_count_); - for (std::size_t voxel_x = 0; voxel_x < suction_cols_count_; ++voxel_x) - { - suction_voxels_[voxel_y][voxel_x] = std::make_shared<SuctionVoxel>( - Eigen::Vector3d(-active_suction_range_x_ / 2.0 + voxel_x_width_ * (voxel_x + 0.5), - -active_suction_range_y_ / 2.0 + voxel_y_width_ * (voxel_y + 0.5), 0), - voxel_x_width_, voxel_y_width_); - } - } - } - - // \brief - get the voxel at the index location [row, col] with [0, 0] being the bottom left - bool getSuctionVoxel(std::size_t row, std::size_t col, std::shared_ptr<SuctionVoxel>& voxel) - { - if (row >= suction_rows_count_) - { - ROS_DEBUG_STREAM_NAMED("suction_voxel_matrix", "Invalid row " << row << "/" << suction_rows_count_ - 1); - return false; - } - - if (col >= suction_cols_count_) - { - ROS_DEBUG_STREAM_NAMED("suction_voxel_matrix", "Invalid col " << col << "/" << suction_cols_count_ - 1); - return false; - } - - voxel = suction_voxels_[row][col]; - return true; - } - - /** \brief Get the voxel at the index i where columns are the minor axis and rows are the major axes. - * @param index - the index of the suction voxel where index / #cols is the row and index % #cols is the col - * index 0 is bottom left - */ - bool getSuctionVoxel(std::size_t index, std::shared_ptr<SuctionVoxel>& voxel) - { - return getSuctionVoxel(index / suction_cols_count_, index % suction_cols_count_, voxel); - } - - std::size_t getNumRows() - { - return suction_rows_count_; - } - - std::size_t getNumCols() - { - return suction_cols_count_; - } - - std::size_t getNumVoxels() - { - return suction_cols_count_ * suction_rows_count_; - } - - double getVoxelArea() - { - return voxel_x_width_ * voxel_y_width_; - } - - double getVoxelWidthX() - { - return voxel_x_width_; - } - - double getVoxelWidthY() - { - return voxel_y_width_; - } - - double getActiveSuctionWidthX() - { - return active_suction_range_x_; - } - - double getActiveSuctionWidthY() - { - return active_suction_range_y_; - } - -protected: - std::size_t suction_rows_count_; - std::size_t suction_cols_count_; - double voxel_x_width_; - double voxel_y_width_; - double active_suction_range_x_; - double active_suction_range_y_; - std::vector<std::vector<std::shared_ptr<SuctionVoxel>>> suction_voxels_; -}; - -} // namespace moveit_grasps - -#endif diff --git a/src/mtc/include/two_finger_grasp_data.h b/src/mtc/include/two_finger_grasp_data.h deleted file mode 100644 index 3b9a00f7493977e5c8bd2040b07eb83bdfdd6bfc..0000000000000000000000000000000000000000 --- a/src/mtc/include/two_finger_grasp_data.h +++ /dev/null @@ -1,120 +0,0 @@ -/* - * Software License Agreement (Modified BSD License) - * - * Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Univ of CO, Boulder, PAL Robotics, S.L. - * nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific - * prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/* Authors: Dave Coleman, Bence Magyar - Description: Data class used by the grasp generator. -*/ - -#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_DATA_H_ -#define MOVEIT_GRASPS__TWO_FINGER_GRASP_DATA_H_ - -// moveit grasps -#include "../include/grasp_data.h" - -namespace moveit_grasps -{ -MOVEIT_CLASS_FORWARD(TwoFingerGraspData); - -class TwoFingerGraspData : public GraspData -{ -public: - /** - * \brief Creates a two finger grasp data object - * \param node_handle - allows for namespacing - * \param end_effector name - which side of a two handed robot to load data for. should correspond to SRDF EE names - * \param robot_model - The robot model - */ - TwoFingerGraspData(const ros::NodeHandle& nh, const std::string& end_effector, - const moveit::core::RobotModelConstPtr& robot_model); - - /** - * \brief Helper function for constructor, loads grasp data from a yaml file (load from roslaunch) - * \param nh - node handle allows for namespacing - * \param end_effector - The end effector joint group name - * \return true on success - */ - bool loadGraspData(const ros::NodeHandle& nh, const std::string& end_effector) override; - - /** - * \brief Set the width between fingers as a percentage of object size and max finger width - * \param fraction_open - [0,1] the 0->closed 1->open - * \return true on success - */ - bool setGraspWidth(double fraction_open, double min_finger_width, trajectory_msgs::JointTrajectory& grasp_posture); - - /** - * \brief Convert width between fingers to joint positions - * \param distance_btw_fingers - (meters) The target distance between the fingers - * \param grasp_posture - output. A joint trajectory with the values for the end effector filled in. - * Note: we interpolate to get the joint value assuming a linear relationship between min and max - * finger distance and min and max finger joint value. This could likely be improved on a per-robot basis. - * \param end_effector - The end effector joint group name - * \return true on success - */ - bool fingerWidthToGraspPosture(double distance_btw_fingers, trajectory_msgs::JointTrajectory& grasp_posture); - - /** - * \brief Convert joint positions to full grasp posture - * \param joint_positions - the full joint state as a vector of doubles - * \param grasp_posture - output. The full grasp posture - * \return true on success - */ - bool jointPositionsToGraspPosture(const std::vector<double>& joint_positions, - trajectory_msgs::JointTrajectory& grasp_posture); - - /** - * \brief Debug data to console - */ - void print() override; - -public: - ///////////////////////////////////// - // Finger gripper specific parameters - ///////////////////////////////////// - // Maximum allowed finger width for a grasp. - // This value should be considerably smaller than max_finger_width - // to allow padded collision checks - double max_grasp_width_; - // Maximum / Minimum distance between fingers - // For calculating the ratio between the distance between fingers and the joint values - double max_finger_width_; - double min_finger_width_; - // Parameter used to ensure generated grasps will overlap object - double gripper_finger_width_; -}; - -} // namespace moveit_grasps - -#endif diff --git a/src/mtc/include/two_finger_grasp_filter.h b/src/mtc/include/two_finger_grasp_filter.h deleted file mode 100644 index 1ec06ce7ee6e92956ecb24422cbcaf2541ac64a1..0000000000000000000000000000000000000000 --- a/src/mtc/include/two_finger_grasp_filter.h +++ /dev/null @@ -1,81 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman <dave@picknik.ai> - Desc: Filters grasps based on kinematic feasibility and collision -*/ - -#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_FILTER_ -#define MOVEIT_GRASPS__TWO_FINGER_GRASP_FILTER_ - -// Parent class -#include "../include/grasp_filter.h" - -// Grasping -#include "../include/two_finger_grasp_generator.h" - -namespace moveit_grasps -{ -class TwoFingerGraspFilter : public GraspFilter -{ -public: - // Constructor - TwoFingerGraspFilter(const robot_state::RobotStatePtr& robot_state, - const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools); - - /** - * \brief Thread for checking part of the possible grasps list - */ - bool processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct) override; - - /** - * \brief Check if ik solution is in collision with fingers closed - * \return true on success - */ - bool checkFingersClosedIK(std::vector<double>& ik_solution, const IkThreadStructPtr& ik_thread_struct, - GraspCandidatePtr& grasp_candidate, - const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const; - -protected: - // Name for logging - const std::string name_; - -}; // end of class - -typedef std::shared_ptr<TwoFingerGraspFilter> TwoFingerGraspFilterPtr; -typedef std::shared_ptr<const TwoFingerGraspFilter> TwoFingerGraspFilterConstPtr; - -} // namespace moveit_grasps - -#endif diff --git a/src/mtc/include/two_finger_grasp_generator.h b/src/mtc/include/two_finger_grasp_generator.h deleted file mode 100644 index 454510aac1cef921e5677c09f7b3d09773738389..0000000000000000000000000000000000000000 --- a/src/mtc/include/two_finger_grasp_generator.h +++ /dev/null @@ -1,257 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy - Desc: Generates geometric grasps for cuboids and blocks, not using physics or contact wrenches -*/ - -#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_GENERATOR_H_ -#define MOVEIT_GRASPS__TWO_FINGER_GRASP_GENERATOR_H_ - -#include "../include/grasp_generator.h" -#include "../include/grasp_candidate.h" -#include "../include/two_finger_grasp_scorer.h" -#include "../include/two_finger_grasp_data.h" - -// Testing -#include <gtest/gtest.h> - -namespace moveit_grasps -{ -struct TwoFingerGraspCandidateConfig -{ - TwoFingerGraspCandidateConfig(); - void enableAllGraspTypes(); - void enableAllGraspAxes(); - void enableAll(); - void disableAllGraspTypes(); - void disableAllGraspAxes(); - void disableAll(); - - /////////////////////////////// - // Finger Gripper config values - /////////////////////////////// - bool enable_corner_grasps_; - bool enable_face_grasps_; - bool enable_variable_angle_grasps_; - bool enable_edge_grasps_; - bool generate_x_axis_grasps_; - bool generate_y_axis_grasps_; - bool generate_z_axis_grasps_; -}; - -class TwoFingerGraspGenerator : public GraspGenerator -{ -public: - // Eigen requires 128-bit alignment for the Eigen::Vector2d's array (of 2 doubles). - // With GCC, this is done with a attribute ((aligned(16))). - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** - * \brief Constructor - */ - TwoFingerGraspGenerator(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, bool verbose = false); - - /* brief sets internal grasp_candidate_config_ variable - * \param grasp_candidate_config - a config describing the grasps to be generated - */ - void setGraspCandidateConfig(const TwoFingerGraspCandidateConfig& grasp_candidate_config); - - /** - * \brief Create possible grasp positions around a cuboid - * \param cuboid_pose - centroid of object to grasp in world frame - * \param depth length of cuboid along local x-axis - * \param width length of cuboid along local y-axis - * \param height length of cuboid along local z-axis - * \param grasp_data data describing end effector - * \param grasp_candidate_config parameter for selectively enabling and disabling different grasp types - * \param grasp_candidates possible grasps generated - * \return true if successful - */ - bool generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height, - const GraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates) override; - - bool generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height, - const TwoFingerGraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates); - - /** - * \brief Setter for grasp score weights - */ - void setGraspScoreWeights(const TwoFingerGraspScoreWeightsPtr& grasp_score_weights) - { - auto two_finger_grasp_score_weights = std::make_shared<TwoFingerGraspScoreWeights>(*grasp_score_weights); - grasp_score_weights_ = std::dynamic_pointer_cast<GraspScoreWeights>(two_finger_grasp_score_weights); - } - - /** - * \brief Setter for grasp score weights - */ - const TwoFingerGraspScoreWeightsPtr getGraspScoreWeights() - { - return std::dynamic_pointer_cast<TwoFingerGraspScoreWeights>(grasp_score_weights_); - } - -protected: - /** - * \brief creates grasp messages from the generated grasp poses - * \param grasp_pose_eef_mount - the grasp pose. (Note: this is the pose of the eef mount not the position of the tcp) - * \param grasp_data data describing the end effector - * \param object_pose - pose of object to grasp - * \param object_size - size of object to grasp - * \param object_width - In the case of finger grippers, the width of the object in the dimension betwen the fingers - * \param grasp_candidates - list possible grasps with new grasp appended - * \return true on success - */ - bool addGrasp(const Eigen::Isometry3d& grasp_pose_eef_mount, const TwoFingerGraspDataPtr& grasp_data, - const Eigen::Isometry3d& object_pose, const Eigen::Vector3d& object_size, double object_width, - std::vector<GraspCandidatePtr>& grasp_candidates); - - /** - * \brief Create grasp positions around one axis of a cuboid - * \param cuboid_pose: centroid of object to grasp in world frame - * \param depth: length of cuboid along local x-axis - * \param width: length of cuboid along local y-axis - * \param height: length of cuboid along local z-axis - * \param axis: axis of cuboid to generate grasps along - * \param grasp_data: data describing end effector - * \param grasp_candidates: possible grasps generated - * \param only_edge_grasps: set to true if object is too wide to grap the face in this axis - * \return true if successful - */ - bool generateCuboidAxisGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height, - grasp_axis_t axis, const TwoFingerGraspDataPtr& grasp_data, - const TwoFingerGraspCandidateConfig& grasp_candidate_config, - std::vector<GraspCandidatePtr>& grasp_candidates); - - /** - * \brief helper function for adding grasps at corner of cuboid - * \param pose - pose of the object to grasp - * \param rotation_angles - rotation angles to go from cuboid pose to standard grasping pose - * \param translation - translation to go from cuboid centroid to grasping location - * \param corner_rotation - extra rotatation needed to align grasp pose as you move around the cuboid - * \param num_radial_grasps - the number of grasps to generate around the corner - * \param grasp_poses_tcp - list of grasp poses generated - * \return the number of poses generated - */ - std::size_t addCornerGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3], - const Eigen::Vector3d& translation, double corner_rotation, - std::size_t num_radial_grasps, EigenSTL::vector_Isometry3d& grasp_poses_tcp); - - /** - * \brief helper function for adding grasps along the face of a cuboid - * \param pose - pose of the object to grasp - * \param rotation_angles - rotation angles to go from cuboid pose to standard grasping pose - * \param delta - distance to move away from cuboid at each step - * \param translation - translation to go from cuboid centroid to grasping location - * \param alignment_rotation - extra rotatation needed to align grasp pose as you move around the cuboid - * \param num_grasps - the number of grasps to generate around the corner - * \param grasp_poses_tcp - list of grasp poses generated - * \return the number of poses generated - */ - std::size_t addFaceGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3], - const Eigen::Vector3d& translation, const Eigen::Vector3d& delta, - double alignment_rotation, std::size_t num_grasps, - EigenSTL::vector_Isometry3d& grasp_poses_tcp); - - /** - * \brief helper function for adding grasps along the edges of the cuboid - * \param pose - pose of the object to grasp - * \param rotation_angles - rotation angles to go from cuboid pose to standard grasping pose - * \param delta - distance to move away from cuboid at each step - * \param translation - translation to go from cuboid centroid to grasping location - * \param alignment_rotation - extra rotatation needed to align grasp pose as you move around the cuboid - * \param num_grasps - the number of grasps to generate around the corner - * \param grasp_poses_tcp - list of grasp poses generated - * \return the number of poses generated - */ - std::size_t addEdgeGraspsHelper(const Eigen::Isometry3d& cuboid_pose, double rotation_angles[3], - const Eigen::Vector3d& translation, const Eigen::Vector3d& delta, - double alignment_rotation, std::size_t num_grasps, - EigenSTL::vector_Isometry3d& grasp_poses_tcp, double corner_rotation); - - /** - * \brief helper function for determining if the grasp will intersect the cuboid - * \param cuboid_pose - centroid of object to grasp in world frame - * \param depth - size of cuboid along x axis - * \param width - size of cuboid along y axis - * \param height - size of cuboid along z axis - * \param grasp_pose_tcp - pose of grasp - * \param grasp_data - data describing end effector - * \return true if the grasp intersects the cuboid - */ - bool graspIntersectionHelper(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height, - const Eigen::Isometry3d& grasp_pose_tcp, const TwoFingerGraspDataPtr& grasp_data); - - /** - * \brief helper function to test intersection of a line with a plane - * \param t - parametric distance along grasp line - * \param u1, v1, u2, v2 - (u,v) coordinates of the line - * \param a, b - length and width of the plane area in which to test for intersection - * \param u, v - location of intersection - * \return true if the line intersects the plane - */ - bool intersectionHelper(double t, double u1, double v1, double u2, double v2, double a, double b, double& u, - double& v); - - /** - * \brief Score the generated finger grasp poses - * \param grasp_pose_tcp - the grasp pose of the tcp - * \param grasp_data - data describing the end effector - * \param object_pose - the pose of the object being grasped - * \param percent_open - percentage that the grippers are open. 0.0 -> grippers are at object width + padding - * \return a score with positive being better - */ - double scoreFingerGrasp(const Eigen::Isometry3d& grasp_pose_tcp, const TwoFingerGraspDataPtr& grasp_data, - const Eigen::Isometry3d& object_pose, double percent_open); - bool - generateFingerGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, double height, - const TwoFingerGraspDataPtr& grasp_data, std::vector<GraspCandidatePtr>& grasp_candidates, - const TwoFingerGraspCandidateConfig& grasp_candidate_config = TwoFingerGraspCandidateConfig()); - -protected: - TwoFingerGraspCandidateConfig grasp_candidate_config_; - - // Tests - FRIEND_TEST(TwoFingerGraspGeneratorTest, GenerateFaceGrasps); - FRIEND_TEST(TwoFingerGraspGeneratorTest, GenerateEdgeGrasps); - FRIEND_TEST(TwoFingerGraspGeneratorTest, GenerateCornerGrasps); - -}; // end of class - -typedef std::shared_ptr<TwoFingerGraspGenerator> TwoFingerGraspGeneratorPtr; -typedef std::shared_ptr<const TwoFingerGraspGenerator> TwoFingerGraspGeneratorConstPtr; - -} // namespace moveit_grasps - -#endif diff --git a/src/mtc/include/two_finger_grasp_scorer.h b/src/mtc/include/two_finger_grasp_scorer.h deleted file mode 100644 index 629cc9d55696a13240b7ffa58ea92c3398597995..0000000000000000000000000000000000000000 --- a/src/mtc/include/two_finger_grasp_scorer.h +++ /dev/null @@ -1,104 +0,0 @@ -/********************************************************************* - * Software License Agreement ("Modified BSD License") - * - * Copyright (c) 2014, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ -/** - * Authors : Andy McEvoy - * Desc : Functions for scoring generated grasps - */ - -#ifndef MOVEIT_GRASPS__TWO_FINGER_GRASP_SCORER_ -#define MOVEIT_GRASPS__TWO_FINGER_GRASP_SCORER_ - -#include "../include/two_finger_grasp_data.h" -#include "../include/grasp_scorer.h" - -namespace moveit_grasps -{ -struct TwoFingerGraspScoreWeights : public GraspScoreWeights -{ - TwoFingerGraspScoreWeights() : GraspScoreWeights(), depth_score_weight_(1.0), width_score_weight_(1.0) - { - } - - /* \brief Compute the weighted score given the orientation and translation scores */ - double computeScore(const Eigen::Vector3d& orientation_scores, const Eigen::Vector3d& translation_scores, - double depth_score, double width_score, bool verbose = false) const; - - /* \brief returns the sum of the grasp score weights*/ - double getWeightTotal() const override; - - // Finger gripper specific weights - double depth_score_weight_; - double width_score_weight_; -}; -// Create smart pointers for this class -typedef std::shared_ptr<TwoFingerGraspScoreWeights> TwoFingerGraspScoreWeightsPtr; - -class TwoFingerGraspScorer : public GraspScorer -{ -public: - /** - * \brief Scores the grasp on how wide the fingers are on approach, the more open the better - * \param grasp_data - pointer to grasp info - * \param percent_open - amount the gripper is open - * 0.0 -> gripper is open to the object width + minimum padding - * 1.0 -> gripper is in full open position - * \return the unweighted score: - * 1.0 -> gripper is wide open, - * 0.0 -> gripper is at minimum position. - */ - static double scoreGraspWidth(const TwoFingerGraspDataPtr& grasp_data, double percent_open); - - /** - * \brief Score the grasp based on how far the object is from the palm of the hand - * \param grasp_pose_tcpgrasp_pose_tcp - the pose of the end effector (not the eef mount) - * \param grasp_data - pointer to grasp info - * \param object_pose - the pose of the object being grasped - * \param max_grasp_distance - the maximum acceptable distance from palm - * \return the unweighted score: - * 1.0 -> object pose and grasp pose have same translation values - * 0.0 -> object is at max distanct - * < 0.0 -> object is beyond the max_grasp_distance - */ - // DEV NOTE: when this function is called we've lost the references to the acutal size of the object. - // max_distance should be the length of the fingers minus some minimum amount that the fingers need to grip an object - // since we don't know the distance from the centoid of the object to the edge of the object, this is set as an - // arbitrary number given our target object set (i.e. I based it off of the cheese it box) - static double scoreDistanceToPalm(const Eigen::Isometry3d& grasp_pose_tcp, const TwoFingerGraspDataPtr& grasp_data, - const Eigen::Isometry3d& object_pose, double min_grasp_distance, - double max_grasp_distance); -}; - -} // namespace moveit_grasps - -#endif diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch index f8795d144b6153e7c8120e2ac417151b2a6dc4cb..20c1eae84471b6e8c23405a8c955072a13cf3cb2 100644 --- a/src/mtc/launch/cell_routine.launch +++ b/src/mtc/launch/cell_routine.launch @@ -1,6 +1,6 @@ <launch> <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> --> - <rosparam command="load" file="$(find mtc)/results/dummy.yaml"/> + <rosparam command="load" file="$(find mtc)/results/dummy/-151902566.yaml"/> <rosparam command="load" file="$(find mtc)/mtc_templates/dummy.yaml" /> <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/> <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/> diff --git a/src/mtc/results/dummy.yaml b/src/mtc/results/dummy.yaml index 772e4c441b143bcafb3e869790381a6a8ec43a6d..d93ffb798fc40b538a830d6d7664f60b0be5319c 100644 --- a/src/mtc/results/dummy.yaml +++ b/src/mtc/results/dummy.yaml @@ -7,7 +7,7 @@ { 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_table_top', 'pos': { 'x': -0.000002 , 'y': 0.099998 , 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } }, +{ 'id' : 'table1_table_top', 'pos': { 'x': -0.000002 , 'y': 0.099998 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } }, { 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, @@ -16,10 +16,10 @@ { 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_table_top', 'pos': { 'x': -0.000005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.880000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_right_panel', 'pos': { 'x': -0.000007, 'y': 2.057498 , 'z': 0.880000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id' : 'table2_table_top', 'pos': { 'x': -0.000005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.000007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} diff --git a/src/mtc/results/dummy/676899603.yaml b/src/mtc/results/dummy/-1058266167.yaml similarity index 77% rename from src/mtc/results/dummy/676899603.yaml rename to src/mtc/results/dummy/-1058266167.yaml index c47d0d5c9494aeb2219c20e8d86893def63bb210..21659a8640fde928342a3489fdfe95cc146b3b31 100644 --- a/src/mtc/results/dummy/676899603.yaml +++ b/src/mtc/results/dummy/-1058266167.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.405000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.405000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/663611616.yaml b/src/mtc/results/dummy/-1071524157.yaml similarity index 82% rename from src/mtc/results/dummy/663611616.yaml rename to src/mtc/results/dummy/-1071524157.yaml index df44868512301dd29b8b1be2a73f4bdb163eb3a0..85026aa8fa004acf6ef8eb1205249002af636127 100644 --- a/src/mtc/results/dummy/663611616.yaml +++ b/src/mtc/results/dummy/-1071524157.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.100005, 'y': 1.184998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1454326127.yaml b/src/mtc/results/dummy/-107328901.yaml similarity index 77% rename from src/mtc/results/dummy/1454326127.yaml rename to src/mtc/results/dummy/-107328901.yaml index 2c6ccac16c046684a5ba74e5ebf1363520d29116..6717690519df6eb4333c93d8a896f990b38552b6 100644 --- a/src/mtc/results/dummy/1454326127.yaml +++ b/src/mtc/results/dummy/-107328901.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.100005, 'y': 0.984998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/605226781.yaml b/src/mtc/results/dummy/-1083227167.yaml similarity index 82% rename from src/mtc/results/dummy/605226781.yaml rename to src/mtc/results/dummy/-1083227167.yaml index 5e36b135833721911b073309d37908a5036f6977..3b44962e51121df05778c0bbe94e2dd2bdcb38fd 100644 --- a/src/mtc/results/dummy/605226781.yaml +++ b/src/mtc/results/dummy/-1083227167.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/630149674.yaml b/src/mtc/results/dummy/-1105096712.yaml similarity index 77% rename from src/mtc/results/dummy/630149674.yaml rename to src/mtc/results/dummy/-1105096712.yaml index c47d0d5c9494aeb2219c20e8d86893def63bb210..21659a8640fde928342a3489fdfe95cc146b3b31 100644 --- a/src/mtc/results/dummy/630149674.yaml +++ b/src/mtc/results/dummy/-1105096712.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.405000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.405000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/616917843.yaml b/src/mtc/results/dummy/-1118397307.yaml similarity index 82% rename from src/mtc/results/dummy/616917843.yaml rename to src/mtc/results/dummy/-1118397307.yaml index df44868512301dd29b8b1be2a73f4bdb163eb3a0..85026aa8fa004acf6ef8eb1205249002af636127 100644 --- a/src/mtc/results/dummy/616917843.yaml +++ b/src/mtc/results/dummy/-1118397307.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.100005, 'y': 1.184998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/651836672.yaml b/src/mtc/results/dummy/-1130182242.yaml similarity index 82% rename from src/mtc/results/dummy/651836672.yaml rename to src/mtc/results/dummy/-1130182242.yaml index 5e36b135833721911b073309d37908a5036f6977..3b44962e51121df05778c0bbe94e2dd2bdcb38fd 100644 --- a/src/mtc/results/dummy/651836672.yaml +++ b/src/mtc/results/dummy/-1130182242.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/423704891.yaml b/src/mtc/results/dummy/-1164235259.yaml similarity index 73% rename from src/mtc/results/dummy/423704891.yaml rename to src/mtc/results/dummy/-1164235259.yaml index 3d75735f83186b62466beec659e599b656b526c1..a04dbcbc3e3dd34fbcef080bce1364dce6373951 100644 --- a/src/mtc/results/dummy/423704891.yaml +++ b/src/mtc/results/dummy/-1164235259.yaml @@ -17,11 +17,11 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.200005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.200004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.452498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.200000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.200007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.452495, 'y': 1.305000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.200004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.452498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.200000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.200007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.452495, 'y': 1.305000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.420002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.420005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/568954925.yaml b/src/mtc/results/dummy/-1166700698.yaml similarity index 77% rename from src/mtc/results/dummy/568954925.yaml rename to src/mtc/results/dummy/-1166700698.yaml index 5258e24fe52a438bc2f01f92e74c486475db060b..b091ee37325ff8f59b7ce9536d852b95733b6cad 100644 --- a/src/mtc/results/dummy/568954925.yaml +++ b/src/mtc/results/dummy/-1166700698.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.305000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.305000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/509248143.yaml b/src/mtc/results/dummy/-1179960295.yaml similarity index 82% rename from src/mtc/results/dummy/509248143.yaml rename to src/mtc/results/dummy/-1179960295.yaml index a7d67f3832226a389c33ae37444a07e49b1f8ac2..898018ca8c01badddf7b1fcd199d6e6d33dda0ab 100644 --- a/src/mtc/results/dummy/509248143.yaml +++ b/src/mtc/results/dummy/-1179960295.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.100005, 'y': 1.084998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/461556082.yaml b/src/mtc/results/dummy/-118381120.yaml similarity index 77% rename from src/mtc/results/dummy/461556082.yaml rename to src/mtc/results/dummy/-118381120.yaml index 7a58307491417a7ec4e0605b6116981035370a1d..53a53d49ba65050a619761b22b42c434c8463c74 100644 --- a/src/mtc/results/dummy/461556082.yaml +++ b/src/mtc/results/dummy/-118381120.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.205000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.204997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/544096359.yaml b/src/mtc/results/dummy/-1191626777.yaml similarity index 82% rename from src/mtc/results/dummy/544096359.yaml rename to src/mtc/results/dummy/-1191626777.yaml index b5c4d9a7538e6cdb036f1c272164389baf121b21..1e80257b5098d9dc76ea08b6e4db4dc121304c74 100644 --- a/src/mtc/results/dummy/544096359.yaml +++ b/src/mtc/results/dummy/-1191626777.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/466931166.yaml b/src/mtc/results/dummy/-1213000891.yaml similarity index 73% rename from src/mtc/results/dummy/466931166.yaml rename to src/mtc/results/dummy/-1213000891.yaml index 3d75735f83186b62466beec659e599b656b526c1..a04dbcbc3e3dd34fbcef080bce1364dce6373951 100644 --- a/src/mtc/results/dummy/466931166.yaml +++ b/src/mtc/results/dummy/-1213000891.yaml @@ -17,11 +17,11 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.200005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.200004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.452498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.200000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.200007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.452495, 'y': 1.305000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.200004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.452498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.200000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.200007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.452495, 'y': 1.305000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.420002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.420005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/522407406.yaml b/src/mtc/results/dummy/-1213390021.yaml similarity index 77% rename from src/mtc/results/dummy/522407406.yaml rename to src/mtc/results/dummy/-1213390021.yaml index 5258e24fe52a438bc2f01f92e74c486475db060b..b091ee37325ff8f59b7ce9536d852b95733b6cad 100644 --- a/src/mtc/results/dummy/522407406.yaml +++ b/src/mtc/results/dummy/-1213390021.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.305000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.305000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/555705912.yaml b/src/mtc/results/dummy/-1226601008.yaml similarity index 82% rename from src/mtc/results/dummy/555705912.yaml rename to src/mtc/results/dummy/-1226601008.yaml index a7d67f3832226a389c33ae37444a07e49b1f8ac2..898018ca8c01badddf7b1fcd199d6e6d33dda0ab 100644 --- a/src/mtc/results/dummy/555705912.yaml +++ b/src/mtc/results/dummy/-1226601008.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.100005, 'y': 1.084998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/497619452.yaml b/src/mtc/results/dummy/-1238289141.yaml similarity index 82% rename from src/mtc/results/dummy/497619452.yaml rename to src/mtc/results/dummy/-1238289141.yaml index b5c4d9a7538e6cdb036f1c272164389baf121b21..1e80257b5098d9dc76ea08b6e4db4dc121304c74 100644 --- a/src/mtc/results/dummy/497619452.yaml +++ b/src/mtc/results/dummy/-1238289141.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/336691910.yaml b/src/mtc/results/dummy/-1256829190.yaml similarity index 73% rename from src/mtc/results/dummy/336691910.yaml rename to src/mtc/results/dummy/-1256829190.yaml index 37e2de46e32fd8084fd4ff590d39e970ad789a4d..e5de7a3d4a2ff2e89db1882834d877f3db46fc62 100644 --- a/src/mtc/results/dummy/336691910.yaml +++ b/src/mtc/results/dummy/-1256829190.yaml @@ -17,11 +17,11 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.086267, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.029400, 'y': 1.950050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.563750, 'y': 1.243165 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.029400, 'y': 1.950050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.563750, 'y': 1.243165 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.019172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.305430, 'y': 1.319206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/324430568.yaml b/src/mtc/results/dummy/-1269289091.yaml similarity index 77% rename from src/mtc/results/dummy/324430568.yaml rename to src/mtc/results/dummy/-1269289091.yaml index 19a88f6f93eecf076cc5e71f77bddd3976a09798..19545d365f9dacad5da715d875f037a389283bea 100644 --- a/src/mtc/results/dummy/324430568.yaml +++ b/src/mtc/results/dummy/-1269289091.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.086267, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.675591 , 'w': 0.737276 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.483532}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.029400, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.675591, 'w': 0.737276 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': -0.029400, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.675591, 'w': 0.737276 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.019172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.105441, 'y': 1.080869, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.675591, 'w': 0.737276 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/415108929.yaml b/src/mtc/results/dummy/-1274604634.yaml similarity index 77% rename from src/mtc/results/dummy/415108929.yaml rename to src/mtc/results/dummy/-1274604634.yaml index 7a58307491417a7ec4e0605b6116981035370a1d..7bdfa2e17389750602293b23b5cc9f4de4a95629 100644 --- a/src/mtc/results/dummy/415108929.yaml +++ b/src/mtc/results/dummy/-1274604634.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.205000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.205000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.204997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/357177233.yaml b/src/mtc/results/dummy/-1280463185.yaml similarity index 77% rename from src/mtc/results/dummy/357177233.yaml rename to src/mtc/results/dummy/-1280463185.yaml index 0eb0fd862afa54f5ed36ccf401c587880599f3e2..2067add7e30a80957e8603aac116cb1d45444049 100644 --- a/src/mtc/results/dummy/357177233.yaml +++ b/src/mtc/results/dummy/-1280463185.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.086267, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.029400, 'y': 1.950050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.029400, 'y': 1.950050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.019172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.305430, 'y': 1.319206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/448406387.yaml b/src/mtc/results/dummy/-1287907953.yaml similarity index 82% rename from src/mtc/results/dummy/448406387.yaml rename to src/mtc/results/dummy/-1287907953.yaml index 8492c8538d73b1d5b08e0fc056204c3e625ceb88..560e11c9307da4ed23871b12d4255eaa453c0637 100644 --- a/src/mtc/results/dummy/448406387.yaml +++ b/src/mtc/results/dummy/-1287907953.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.100005, 'y': 0.984998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/436801234.yaml b/src/mtc/results/dummy/-1299660620.yaml similarity index 82% rename from src/mtc/results/dummy/436801234.yaml rename to src/mtc/results/dummy/-1299660620.yaml index b694cc6f6ed60cee97549e89a1cb978a88b72dff..7b7da08be213b300474e31c5b3c9c8e6984e6f6b 100644 --- a/src/mtc/results/dummy/436801234.yaml +++ b/src/mtc/results/dummy/-1299660620.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.204997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/380238980.yaml b/src/mtc/results/dummy/-1301462009.yaml similarity index 73% rename from src/mtc/results/dummy/380238980.yaml rename to src/mtc/results/dummy/-1301462009.yaml index 37e2de46e32fd8084fd4ff590d39e970ad789a4d..e5de7a3d4a2ff2e89db1882834d877f3db46fc62 100644 --- a/src/mtc/results/dummy/380238980.yaml +++ b/src/mtc/results/dummy/-1301462009.yaml @@ -17,11 +17,11 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.086267, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.029400, 'y': 1.950050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.563750, 'y': 1.243165 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.029400, 'y': 1.950050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.563750, 'y': 1.243165 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.019172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.305430, 'y': 1.319206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/368021780.yaml b/src/mtc/results/dummy/-1313991264.yaml similarity index 77% rename from src/mtc/results/dummy/368021780.yaml rename to src/mtc/results/dummy/-1313991264.yaml index 19a88f6f93eecf076cc5e71f77bddd3976a09798..19545d365f9dacad5da715d875f037a389283bea 100644 --- a/src/mtc/results/dummy/368021780.yaml +++ b/src/mtc/results/dummy/-1313991264.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.086267, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.675591 , 'w': 0.737276 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.483532}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.029400, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.675591, 'w': 0.737276 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': -0.029400, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.675591, 'w': 0.737276 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.019172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.105441, 'y': 1.080869, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.675591, 'w': 0.737276 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/-1321395857.yaml b/src/mtc/results/dummy/-1321395857.yaml new file mode 100644 index 0000000000000000000000000000000000000000..7bdfa2e17389750602293b23b5cc9f4de4a95629 --- /dev/null +++ b/src/mtc/results/dummy/-1321395857.yaml @@ -0,0 +1,26 @@ +{ 'objects' : [ +{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': -0.100002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } }, +{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.205000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, +{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.204997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } +]} \ No newline at end of file diff --git a/src/mtc/results/dummy/313407848.yaml b/src/mtc/results/dummy/-1325476019.yaml similarity index 77% rename from src/mtc/results/dummy/313407848.yaml rename to src/mtc/results/dummy/-1325476019.yaml index 0eb0fd862afa54f5ed36ccf401c587880599f3e2..2067add7e30a80957e8603aac116cb1d45444049 100644 --- a/src/mtc/results/dummy/313407848.yaml +++ b/src/mtc/results/dummy/-1325476019.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.086267, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.029400, 'y': 1.950050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': -0.056870 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.029400, 'y': 1.950050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.019172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.305430, 'y': 1.319206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/401939748.yaml b/src/mtc/results/dummy/-1334648679.yaml similarity index 82% rename from src/mtc/results/dummy/401939748.yaml rename to src/mtc/results/dummy/-1334648679.yaml index 8492c8538d73b1d5b08e0fc056204c3e625ceb88..560e11c9307da4ed23871b12d4255eaa453c0637 100644 --- a/src/mtc/results/dummy/401939748.yaml +++ b/src/mtc/results/dummy/-1334648679.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.100005, 'y': 0.984998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/390180518.yaml b/src/mtc/results/dummy/-1346543685.yaml similarity index 82% rename from src/mtc/results/dummy/390180518.yaml rename to src/mtc/results/dummy/-1346543685.yaml index b694cc6f6ed60cee97549e89a1cb978a88b72dff..7b7da08be213b300474e31c5b3c9c8e6984e6f6b 100644 --- a/src/mtc/results/dummy/390180518.yaml +++ b/src/mtc/results/dummy/-1346543685.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.204997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1443430664.yaml b/src/mtc/results/dummy/-139322185.yaml similarity index 74% rename from src/mtc/results/dummy/1443430664.yaml rename to src/mtc/results/dummy/-139322185.yaml index 693ce63f5064d8a7dfefe20f25f9a7ddc6b59c41..0752cf3170fb4108781cdc8029436c67bcbe6a42 100644 --- a/src/mtc/results/dummy/1443430664.yaml +++ b/src/mtc/results/dummy/-139322185.yaml @@ -17,10 +17,11 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.205000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.204997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1497853990.yaml b/src/mtc/results/dummy/-151902566.yaml similarity index 77% rename from src/mtc/results/dummy/1497853990.yaml rename to src/mtc/results/dummy/-151902566.yaml index 2c6ccac16c046684a5ba74e5ebf1363520d29116..6717690519df6eb4333c93d8a896f990b38552b6 100644 --- a/src/mtc/results/dummy/1497853990.yaml +++ b/src/mtc/results/dummy/-151902566.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.100005, 'y': 0.984998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/-163087766.yaml b/src/mtc/results/dummy/-163087766.yaml new file mode 100644 index 0000000000000000000000000000000000000000..53a53d49ba65050a619761b22b42c434c8463c74 --- /dev/null +++ b/src/mtc/results/dummy/-163087766.yaml @@ -0,0 +1,26 @@ +{ 'objects' : [ +{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': -0.100002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } }, +{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, +{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.204997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } +]} \ No newline at end of file diff --git a/src/mtc/results/dummy/-1692777619.yaml b/src/mtc/results/dummy/-1692777619.yaml deleted file mode 100644 index 6090c39a6e13bb198fbed95b86d73a7cd4f82114..0000000000000000000000000000000000000000 --- a/src/mtc/results/dummy/-1692777619.yaml +++ /dev/null @@ -1,27 +0,0 @@ -{ 'objects' : [ -{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': 0.099998 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } }, -{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.405000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, -{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } -]} \ No newline at end of file diff --git a/src/mtc/results/dummy/-1736477370.yaml b/src/mtc/results/dummy/-1736477370.yaml deleted file mode 100644 index 6090c39a6e13bb198fbed95b86d73a7cd4f82114..0000000000000000000000000000000000000000 --- a/src/mtc/results/dummy/-1736477370.yaml +++ /dev/null @@ -1,27 +0,0 @@ -{ 'objects' : [ -{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': 0.099998 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } }, -{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.405000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, -{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } -]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1461432933.yaml b/src/mtc/results/dummy/-239267840.yaml similarity index 82% rename from src/mtc/results/dummy/1461432933.yaml rename to src/mtc/results/dummy/-239267840.yaml index 6efc5a58618be4ed6aa9205d9738928a4b61159e..9861b7abcf1a2380171622c4a7f5674f6990fd1d 100644 --- a/src/mtc/results/dummy/1461432933.yaml +++ b/src/mtc/results/dummy/-239267840.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.000005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.204997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1691132293.yaml b/src/mtc/results/dummy/-26465509.yaml similarity index 77% rename from src/mtc/results/dummy/1691132293.yaml rename to src/mtc/results/dummy/-26465509.yaml index c7d7c84e15828ae6e980e0de13c40f217629141d..48426c6d0d54a0125b3ea72859a613746cc44345 100644 --- a/src/mtc/results/dummy/1691132293.yaml +++ b/src/mtc/results/dummy/-26465509.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.113740, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.087267}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.056871 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': 0.056867 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.170610, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.536277, 'y': 1.356901 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.056871 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': 0.056867 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.170610, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.536277, 'y': 1.356901 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.219165, 'y': -0.019176, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.332903, 'y': 1.280858, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1524066269.yaml b/src/mtc/results/dummy/-288597854.yaml similarity index 82% rename from src/mtc/results/dummy/1524066269.yaml rename to src/mtc/results/dummy/-288597854.yaml index 6efc5a58618be4ed6aa9205d9738928a4b61159e..9861b7abcf1a2380171622c4a7f5674f6990fd1d 100644 --- a/src/mtc/results/dummy/1524066269.yaml +++ b/src/mtc/results/dummy/-288597854.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.000005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.204997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1202468027.yaml b/src/mtc/results/dummy/-540664156.yaml similarity index 82% rename from src/mtc/results/dummy/1202468027.yaml rename to src/mtc/results/dummy/-540664156.yaml index bba400b311ac9e525f99b6cefa5187af86e911ad..cb62992cd4b5c4bd51f66897923e42b77d02c4b4 100644 --- a/src/mtc/results/dummy/1202468027.yaml +++ b/src/mtc/results/dummy/-540664156.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.000005, 'y': 1.104998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.757498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.757498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.200003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.104997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1191311555.yaml b/src/mtc/results/dummy/-551819248.yaml similarity index 82% rename from src/mtc/results/dummy/1191311555.yaml rename to src/mtc/results/dummy/-551819248.yaml index a1457703e8c3b9112b631eb6f267828dfe529d94..71561f7c6799d0a4ae1826dc26a44591d8a91077 100644 --- a/src/mtc/results/dummy/1191311555.yaml +++ b/src/mtc/results/dummy/-551819248.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.000006, 'y': 1.399998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.000008, 'y': 2.052498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.000008, 'y': 2.052498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.200003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220006, 'y': 1.399997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1150568114.yaml b/src/mtc/results/dummy/-587407747.yaml similarity index 82% rename from src/mtc/results/dummy/1150568114.yaml rename to src/mtc/results/dummy/-587407747.yaml index bba400b311ac9e525f99b6cefa5187af86e911ad..cb62992cd4b5c4bd51f66897923e42b77d02c4b4 100644 --- a/src/mtc/results/dummy/1150568114.yaml +++ b/src/mtc/results/dummy/-587407747.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.000005, 'y': 1.104998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.757498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.757498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.200003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.104997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1138642515.yaml b/src/mtc/results/dummy/-598701477.yaml similarity index 82% rename from src/mtc/results/dummy/1138642515.yaml rename to src/mtc/results/dummy/-598701477.yaml index a1457703e8c3b9112b631eb6f267828dfe529d94..71561f7c6799d0a4ae1826dc26a44591d8a91077 100644 --- a/src/mtc/results/dummy/1138642515.yaml +++ b/src/mtc/results/dummy/-598701477.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.000006, 'y': 1.399998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.000008, 'y': 2.052498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.452498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.852502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.000008, 'y': 2.052498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.200003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220006, 'y': 1.399997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1740918153.yaml b/src/mtc/results/dummy/-73418491.yaml similarity index 77% rename from src/mtc/results/dummy/1740918153.yaml rename to src/mtc/results/dummy/-73418491.yaml index c7d7c84e15828ae6e980e0de13c40f217629141d..48426c6d0d54a0125b3ea72859a613746cc44345 100644 --- a/src/mtc/results/dummy/1740918153.yaml +++ b/src/mtc/results/dummy/-73418491.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.113740, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.087267}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.056871 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': 0.056867 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.170610, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.536277, 'y': 1.356901 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.056871 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': 0.056867 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043619 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.170610, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.536277, 'y': 1.356901 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.219165, 'y': -0.019176, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.332903, 'y': 1.280858, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043619, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1487024111.yaml b/src/mtc/results/dummy/-94891628.yaml similarity index 74% rename from src/mtc/results/dummy/1487024111.yaml rename to src/mtc/results/dummy/-94891628.yaml index 693ce63f5064d8a7dfefe20f25f9a7ddc6b59c41..0752cf3170fb4108781cdc8029436c67bcbe6a42 100644 --- a/src/mtc/results/dummy/1487024111.yaml +++ b/src/mtc/results/dummy/-94891628.yaml @@ -17,10 +17,11 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.205000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.204997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/613458718.yaml b/src/mtc/results/dummy/-953422313.yaml similarity index 73% rename from src/mtc/results/dummy/613458718.yaml rename to src/mtc/results/dummy/-953422313.yaml index 057ce407328910e1067b078c813b2913e521d31c..0334e12e266abec4112adac9780b894b44b3cc6e 100644 --- a/src/mtc/results/dummy/613458718.yaml +++ b/src/mtc/results/dummy/-953422313.yaml @@ -17,11 +17,11 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.086267, 'y': 1.400032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.750015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': 0.043130 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.550019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.029400, 'y': 2.050050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.563750, 'y': 1.343165 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.750015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': 0.043130 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.550019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.029400, 'y': 2.050050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.563750, 'y': 1.343165 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.119172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.305430, 'y': 1.419206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/657141845.yaml b/src/mtc/results/dummy/-997664293.yaml similarity index 73% rename from src/mtc/results/dummy/657141845.yaml rename to src/mtc/results/dummy/-997664293.yaml index 057ce407328910e1067b078c813b2913e521d31c..0334e12e266abec4112adac9780b894b44b3cc6e 100644 --- a/src/mtc/results/dummy/657141845.yaml +++ b/src/mtc/results/dummy/-997664293.yaml @@ -17,11 +17,11 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.086267, 'y': 1.400032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.750015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': 0.043130 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.550019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.029400, 'y': 2.050050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.563750, 'y': 1.343165 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.143135 , 'y': 0.750015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.450015 , 'y': 0.043130 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.256870 , 'y': -0.550019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.029400, 'y': 2.050050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.563750, 'y': 1.343165 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.419165, 'y': 0.119172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.305430, 'y': 1.419206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/-1759692146.yaml b/src/mtc/results/dummy/1002613186.yaml similarity index 74% rename from src/mtc/results/dummy/-1759692146.yaml rename to src/mtc/results/dummy/1002613186.yaml index d67e3c4c71906fd9ba7df4c7aa6e074162e23078..d875ea561232c2085aee8b14c469e62353a6e48e 100644 --- a/src/mtc/results/dummy/-1759692146.yaml +++ b/src/mtc/results/dummy/1002613186.yaml @@ -17,10 +17,11 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.405000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1876946886.yaml b/src/mtc/results/dummy/106540234.yaml similarity index 82% rename from src/mtc/results/dummy/1876946886.yaml rename to src/mtc/results/dummy/106540234.yaml index 22bb2443cc1e2dc8b8b6f42907d596290de73412..afb02c3a71c8dda4bca214219e75123d70a73114 100644 --- a/src/mtc/results/dummy/1876946886.yaml +++ b/src/mtc/results/dummy/106540234.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.000005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1466555132.yaml b/src/mtc/results/dummy/1466555132.yaml deleted file mode 100644 index d48db4d3ed1891ab4efa5a67b88c0bf54dff4184..0000000000000000000000000000000000000000 --- a/src/mtc/results/dummy/1466555132.yaml +++ /dev/null @@ -1,27 +0,0 @@ -{ 'objects' : [ -{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': -0.100002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } }, -{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.205000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, -{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.204997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } -]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1510066581.yaml b/src/mtc/results/dummy/1510066581.yaml deleted file mode 100644 index d48db4d3ed1891ab4efa5a67b88c0bf54dff4184..0000000000000000000000000000000000000000 --- a/src/mtc/results/dummy/1510066581.yaml +++ /dev/null @@ -1,27 +0,0 @@ -{ 'objects' : [ -{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': -0.100002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } }, -{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.204998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.552498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.752502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.857498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.205000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.100003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, -{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.204997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } -]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1926655009.yaml b/src/mtc/results/dummy/161620969.yaml similarity index 82% rename from src/mtc/results/dummy/1926655009.yaml rename to src/mtc/results/dummy/161620969.yaml index 22bb2443cc1e2dc8b8b6f42907d596290de73412..afb02c3a71c8dda4bca214219e75123d70a73114 100644 --- a/src/mtc/results/dummy/1926655009.yaml +++ b/src/mtc/results/dummy/161620969.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.000005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.000007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/2009643783.yaml b/src/mtc/results/dummy/2009643783.yaml deleted file mode 100644 index 7fd072365c2f94cc7ec468f5c5267d9c0f7469a4..0000000000000000000000000000000000000000 --- a/src/mtc/results/dummy/2009643783.yaml +++ /dev/null @@ -1,27 +0,0 @@ -{ 'objects' : [ -{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': -0.000002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } }, -{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.305000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, -{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } -]} \ No newline at end of file diff --git a/src/mtc/results/dummy/2053173671.yaml b/src/mtc/results/dummy/2053173671.yaml deleted file mode 100644 index 7fd072365c2f94cc7ec468f5c5267d9c0f7469a4..0000000000000000000000000000000000000000 --- a/src/mtc/results/dummy/2053173671.yaml +++ /dev/null @@ -1,27 +0,0 @@ -{ 'objects' : [ -{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': -0.000002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } }, -{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, -{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.305000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, -{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } -]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1998466333.yaml b/src/mtc/results/dummy/241047948.yaml similarity index 77% rename from src/mtc/results/dummy/1998466333.yaml rename to src/mtc/results/dummy/241047948.yaml index a36b32e4230c1eb87db349e37ad89e610572ee66..ca29628255a2e50f3e6412bd5007d73e32729b26 100644 --- a/src/mtc/results/dummy/1998466333.yaml +++ b/src/mtc/results/dummy/241047948.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.113744, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.087269}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.056873 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': 0.056869 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.170614, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.536273, 'y': 1.356903 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.056873 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': 0.056869 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.170614, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.536273, 'y': 1.356903 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.219165, 'y': -0.019177, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.332906, 'y': 1.280857, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/2045084535.yaml b/src/mtc/results/dummy/288860765.yaml similarity index 77% rename from src/mtc/results/dummy/2045084535.yaml rename to src/mtc/results/dummy/288860765.yaml index a36b32e4230c1eb87db349e37ad89e610572ee66..ca29628255a2e50f3e6412bd5007d73e32729b26 100644 --- a/src/mtc/results/dummy/2045084535.yaml +++ b/src/mtc/results/dummy/288860765.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.113744, 'y': 1.300032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.087269}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.056873 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': 0.056869 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.170614, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': 0.536273, 'y': 1.356903 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.056873 , 'y': 0.650015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': 0.056869 , 'y': -0.650019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.043621 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.170614, 'y': 1.950049 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.536273, 'y': 1.356903 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.219165, 'y': -0.019177, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.332906, 'y': 1.280857, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.043621, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/385732106.yaml b/src/mtc/results/dummy/385732106.yaml new file mode 100644 index 0000000000000000000000000000000000000000..90ffb23832d265133057b7ae860243251d991b40 --- /dev/null +++ b/src/mtc/results/dummy/385732106.yaml @@ -0,0 +1,26 @@ +{ 'objects' : [ +{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': -0.000002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } }, +{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, +{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } +]} \ No newline at end of file diff --git a/src/mtc/results/dummy/2040956261.yaml b/src/mtc/results/dummy/396809119.yaml similarity index 77% rename from src/mtc/results/dummy/2040956261.yaml rename to src/mtc/results/dummy/396809119.yaml index 742e17cf7c6f9deed89420c04893d190a8fe7f65..1f886d601223dd38df7addd3bf147b5833dce83e 100644 --- a/src/mtc/results/dummy/2040956261.yaml +++ b/src/mtc/results/dummy/396809119.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.100005, 'y': 1.084998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1986169185.yaml b/src/mtc/results/dummy/409146028.yaml similarity index 74% rename from src/mtc/results/dummy/1986169185.yaml rename to src/mtc/results/dummy/409146028.yaml index 585676d10d50cee56471bff779451efa299c0055..e7c902e83221aa3a4b6810f1d659fd318912f2b8 100644 --- a/src/mtc/results/dummy/1986169185.yaml +++ b/src/mtc/results/dummy/409146028.yaml @@ -17,10 +17,11 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.305000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/429872256.yaml b/src/mtc/results/dummy/429872256.yaml new file mode 100644 index 0000000000000000000000000000000000000000..90ffb23832d265133057b7ae860243251d991b40 --- /dev/null +++ b/src/mtc/results/dummy/429872256.yaml @@ -0,0 +1,26 @@ +{ 'objects' : [ +{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': -0.000002 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } }, +{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, +{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } +]} \ No newline at end of file diff --git a/src/mtc/results/dummy/1997263146.yaml b/src/mtc/results/dummy/440798129.yaml similarity index 77% rename from src/mtc/results/dummy/1997263146.yaml rename to src/mtc/results/dummy/440798129.yaml index 742e17cf7c6f9deed89420c04893d190a8fe7f65..1f886d601223dd38df7addd3bf147b5833dce83e 100644 --- a/src/mtc/results/dummy/1997263146.yaml +++ b/src/mtc/results/dummy/440798129.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.100005, 'y': 1.084998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/2030127988.yaml b/src/mtc/results/dummy/453123235.yaml similarity index 74% rename from src/mtc/results/dummy/2030127988.yaml rename to src/mtc/results/dummy/453123235.yaml index 585676d10d50cee56471bff779451efa299c0055..e7c902e83221aa3a4b6810f1d659fd318912f2b8 100644 --- a/src/mtc/results/dummy/2030127988.yaml +++ b/src/mtc/results/dummy/453123235.yaml @@ -17,10 +17,11 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.304998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.652498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': -0.000000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.652502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 1.957498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.305000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': -0.000003, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.304997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/-1982088882.yaml b/src/mtc/results/dummy/512151887.yaml similarity index 82% rename from src/mtc/results/dummy/-1982088882.yaml rename to src/mtc/results/dummy/512151887.yaml index 83c4b90269a00fa5780b4b3c865eb0f9e6239308..942b30d841fe929fd016d004ab999c48c53d3361 100644 --- a/src/mtc/results/dummy/-1982088882.yaml +++ b/src/mtc/results/dummy/512151887.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.000005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.000007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.000007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/-2028402352.yaml b/src/mtc/results/dummy/568064102.yaml similarity index 82% rename from src/mtc/results/dummy/-2028402352.yaml rename to src/mtc/results/dummy/568064102.yaml index 83c4b90269a00fa5780b4b3c865eb0f9e6239308..942b30d841fe929fd016d004ab999c48c53d3361 100644 --- a/src/mtc/results/dummy/-2028402352.yaml +++ b/src/mtc/results/dummy/568064102.yaml @@ -17,9 +17,9 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.000005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.000007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.000004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.000000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.000007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.220002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.220005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/-1803427587.yaml b/src/mtc/results/dummy/841127331.yaml similarity index 77% rename from src/mtc/results/dummy/-1803427587.yaml rename to src/mtc/results/dummy/841127331.yaml index 410f2ce1dc1ab8d80922cdf36f568b6eebfd1413..3416f0bd137085f6654941e86d57f61fc8335f3c 100644 --- a/src/mtc/results/dummy/-1803427587.yaml +++ b/src/mtc/results/dummy/841127331.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': 0.013733, 'y': 1.400032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.043135 , 'y': 0.750015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.550015 , 'y': 0.043130 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.156870 , 'y': -0.550019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': 0.070600, 'y': 2.050050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.043135 , 'y': 0.750015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.550015 , 'y': 0.043130 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.156870 , 'y': -0.550019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': 0.070600, 'y': 2.050050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.319165, 'y': 0.119172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.205430, 'y': 1.419206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/-1847007484.yaml b/src/mtc/results/dummy/890568488.yaml similarity index 77% rename from src/mtc/results/dummy/-1847007484.yaml rename to src/mtc/results/dummy/890568488.yaml index 410f2ce1dc1ab8d80922cdf36f568b6eebfd1413..3416f0bd137085f6654941e86d57f61fc8335f3c 100644 --- a/src/mtc/results/dummy/-1847007484.yaml +++ b/src/mtc/results/dummy/890568488.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': 0.013733, 'y': 1.400032, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': 0.000000, 'y': -0.087264}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.043135 , 'y': 0.750015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.550015 , 'y': 0.043130 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.156870 , 'y': -0.550019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': 0.070600, 'y': 2.050050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.043135 , 'y': 0.750015 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.550015 , 'y': 0.043130 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.156870 , 'y': -0.550019 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': -0.043618 , 'w': 0.999048 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': 0.070600, 'y': 2.050050 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.319165, 'y': 0.119172, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.205430, 'y': 1.419206, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': -0.043618, 'w': 0.999048 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/934553240.yaml b/src/mtc/results/dummy/934553240.yaml new file mode 100644 index 0000000000000000000000000000000000000000..46cc77d2bee1fac88c1faa83aee5779e848998a3 --- /dev/null +++ b/src/mtc/results/dummy/934553240.yaml @@ -0,0 +1,26 @@ +{ 'objects' : [ +{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': 0.099998 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } }, +{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, +{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } +]} \ No newline at end of file diff --git a/src/mtc/results/dummy/-1748758618.yaml b/src/mtc/results/dummy/945520390.yaml similarity index 77% rename from src/mtc/results/dummy/-1748758618.yaml rename to src/mtc/results/dummy/945520390.yaml index 89536d2e44cbbd41422013ed2afb8c8c5d0a5819..16733ed6240ca861faf511d21a711d45bc15003a 100644 --- a/src/mtc/results/dummy/-1748758618.yaml +++ b/src/mtc/results/dummy/945520390.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.100005, 'y': 1.184998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/-1715933098.yaml b/src/mtc/results/dummy/957818188.yaml similarity index 74% rename from src/mtc/results/dummy/-1715933098.yaml rename to src/mtc/results/dummy/957818188.yaml index d67e3c4c71906fd9ba7df4c7aa6e074162e23078..d875ea561232c2085aee8b14c469e62353a6e48e 100644 --- a/src/mtc/results/dummy/-1715933098.yaml +++ b/src/mtc/results/dummy/957818188.yaml @@ -17,10 +17,11 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': 0.552495, 'y': 1.405000 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/results/dummy/978772872.yaml b/src/mtc/results/dummy/978772872.yaml new file mode 100644 index 0000000000000000000000000000000000000000..46cc77d2bee1fac88c1faa83aee5779e848998a3 --- /dev/null +++ b/src/mtc/results/dummy/978772872.yaml @@ -0,0 +1,26 @@ +{ 'objects' : [ +{ 'id' : 'table1_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table1_table_top', 'pos': { 'x': -0.100002 , 'y': 0.099998 , 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000 , 'p': -0.000000 , 'y': 0.000002 } }, +{ 'id' : 'table2_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_3', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_wheel_4', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_front', 'pos': { 'x': 0,'y': 0, 'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_back', 'pos': { 'x': 0,'y': 0,'z': 0.45 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, +{ 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 0.000003}}, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_right_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.700000,'width': 0.500000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, +{ 'id': 'arm2','type': 'ARM','pos': { 'x': -0.320005, 'y': 1.404997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } +]} \ No newline at end of file diff --git a/src/mtc/results/dummy/-1705045806.yaml b/src/mtc/results/dummy/990058227.yaml similarity index 77% rename from src/mtc/results/dummy/-1705045806.yaml rename to src/mtc/results/dummy/990058227.yaml index 89536d2e44cbbd41422013ed2afb8c8c5d0a5819..16733ed6240ca861faf511d21a711d45bc15003a 100644 --- a/src/mtc/results/dummy/-1705045806.yaml +++ b/src/mtc/results/dummy/990058227.yaml @@ -17,10 +17,10 @@ { 'id' : 'table2_body_left', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_body_right', 'pos': { 'x': 0,'y': 0,'z': 0.50 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, { 'id' : 'table2_table_top', 'pos': { 'x': -0.100005, 'y': 1.404998, 'z': 0.885000 },'size': { 'length': 0.800000 ,'width': 0.800000 ,'height': 0.010000 },'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.707108 , 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }, 'rpy': { 'r': 0.000000, 'p': -0.000000, 'y': 1.570799}}, -{ 'id': 'table1_table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table1_table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, -{ 'id': 'table2_table2_front_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, +{ 'id': 'table1_right_panel' , 'pos': { 'x': -0.100004 , 'y': 0.752498 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_front_panel' , 'pos': { 'x': 0.552498 , 'y': 0.100000 , 'z': 0.885000 } , 'size': { 'length': 0.500000 , 'width': 0.700000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table1_left_panel' , 'pos': { 'x': -0.100000 , 'y': -0.552502 , 'z': 0.885000 } , 'size': { 'length': 0.700000 , 'width': 0.500000 , 'height': 0.010000 } , 'orientation': { 'x': 0.000000 , 'y': 0.000000 , 'z': 0.000001 , 'w': 1.000000 } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, +{ 'id': 'table2_front_panel', 'pos': { 'x': -0.100007, 'y': 2.057498 , 'z': 0.885000 },'size': { 'length': 0.500000,'width': 0.700000,'height': 0.010000 },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, { 'id': 'arm1','type': 'ARM','pos': { 'x': -0.320002, 'y': 0.099997, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.000001, 'w': 1.000000 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }, { 'id': 'arm2','type': 'ARM','pos': { 'x': -0.100005, 'y': 1.184998, 'z': 0.89 },'size': { },'orientation': { 'x': 0.000000, 'y': 0.000000, 'z': 0.707108, 'w': 0.707106 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } } ]} \ No newline at end of file diff --git a/src/mtc/src/base_routine.cpp b/src/mtc/src/base_routine.cpp index 493dac15e36d2d70a87617c1b81ed2f0a52724d9..a898bbc6d497b6790746271baa3f42b8791fe537 100644 --- a/src/mtc/src/base_routine.cpp +++ b/src/mtc/src/base_routine.cpp @@ -81,17 +81,17 @@ int main(int argc, char **argv){ tf2::Vector3 size; tf2::Quaternion rot; + (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0); + (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0); + (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0); + (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ((value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) /2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0); - - (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0); - (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0); - (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0); ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ()); ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW()); @@ -108,17 +108,17 @@ int main(int argc, char **argv){ tf2::Vector3 size; tf2::Quaternion rot; + (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0); + (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0); + (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0); + (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ((value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) /2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0); - - (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0); - (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0); - (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0); ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ()); ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW()); @@ -145,7 +145,7 @@ int main(int argc, char **argv){ (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); @@ -185,7 +185,7 @@ int main(int argc, char **argv){ (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); @@ -227,7 +227,7 @@ int main(int argc, char **argv){ (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); @@ -267,7 +267,7 @@ int main(int argc, char **argv){ (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); @@ -305,7 +305,7 @@ int main(int argc, char **argv){ (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); @@ -343,7 +343,7 @@ int main(int argc, char **argv){ (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); diff --git a/src/mtc/src/cell_routine.cpp b/src/mtc/src/cell_routine.cpp index c5697f3bdd4cd43fc8512e53debedfd15073e953..5ca598cf8979bf8f28ec4c80a297ca0867f843b7 100644 --- a/src/mtc/src/cell_routine.cpp +++ b/src/mtc/src/cell_routine.cpp @@ -72,9 +72,13 @@ for (int i = 0; i < resources.size(); i++){ tf2::Vector3 size; tf2::Quaternion rot; + (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0); + (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0); + (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0); + (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ((value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) /2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); @@ -89,6 +93,7 @@ for (int i = 0; i < resources.size(); i++){ ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ()); std::string name("panda_arm1"); + moveit_mediator->task_map().insert({name, std::vector<moveit::task_constructor::Task>()}); Abstract_robot* robot1 = new Moveit_robot(name, tf2::Transform(rot, pos), size); Moveit_robot* ceti1 = dynamic_cast<Moveit_robot*>(robot1); ceti1->set_observer_mask(wing_config::RML); @@ -102,23 +107,24 @@ for (int i = 0; i < resources.size(); i++){ tf2::Vector3 size; tf2::Quaternion rot; + (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0); + (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0); + (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0); + (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ((value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) /2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0); - - (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0); - (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0); - (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0); ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ()); ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW()); ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ()); std::string name("panda_arm2"); + moveit_mediator->task_map().insert({name, std::vector<moveit::task_constructor::Task>()}); Abstract_robot* robot2 = new Moveit_robot(name, tf2::Transform(rot, pos), size); Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(robot2); ceti2->set_observer_mask(wing_config::RML); @@ -143,7 +149,7 @@ for (int i = 0; i < resources.size(); i++){ (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])+ size.getZ()/2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); @@ -174,7 +180,7 @@ for (int i = 0; i < resources.size(); i++){ (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); @@ -187,7 +193,6 @@ for (int i = 0; i < resources.size(); i++){ a += std::pow(2, 1); blueprints_per_robot[0][1].name_ = std::string("table1_front_panel"); - ROS_INFO("uzguzg"); blueprints_per_robot[0][1].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos); blueprints_per_robot[0][1].size_ = size; ROS_INFO("=> WING:loading SUCCESS"); @@ -205,7 +210,7 @@ for (int i = 0; i < resources.size(); i++){ (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); @@ -235,7 +240,7 @@ for (int i = 0; i < resources.size(); i++){ (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])+ size.getZ()/2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); @@ -264,7 +269,7 @@ for (int i = 0; i < resources.size(); i++){ (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])+ size.getZ()/2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); @@ -295,7 +300,7 @@ for (int i = 0; i < resources.size(); i++){ (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0); (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0); - (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) - size.getZ()/2) :pos.setZ(0); + (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])+ size.getZ()/2) :pos.setZ(0); (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0); (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0); (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0); @@ -316,7 +321,6 @@ for (int i = 0; i < resources.size(); i++){ - ROS_INFO("ndfasofn"); moveit_mediator->set_wings(blueprints_per_robot); Moveit_robot* ceti1 = dynamic_cast<Moveit_robot*>(mediator->robots()[0]); Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(mediator->robots()[1]); diff --git a/src/mtc/src/grasp_candidate.cpp b/src/mtc/src/grasp_candidate.cpp deleted file mode 100644 index 5482843520f118f662762f1e95c05e1afd0c0f2d..0000000000000000000000000000000000000000 --- a/src/mtc/src/grasp_candidate.cpp +++ /dev/null @@ -1,116 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman <dave@picknik.ai> - Desc: Filters grasps based on kinematic feasibility and collision -*/ - -#include "../include/grasp_candidate.h" - -namespace moveit_grasps -{ -GraspCandidate::GraspCandidate(const moveit_msgs::Grasp& grasp, const GraspDataPtr& grasp_data, - const Eigen::Isometry3d& cuboid_pose) - : grasp_(grasp) - , grasp_data_(grasp_data) - , cuboid_pose_(cuboid_pose) - , grasp_filtered_code_(GraspFilterCode::NOT_FILTERED) -{ -} - -bool GraspCandidate::getPreGraspState(moveit::core::RobotStatePtr& robot_state) -{ - // Error check - if (pregrasp_ik_solution_.empty()) - { - ROS_ERROR_STREAM_NAMED("grasp_candidate", "No pregrasp ik solution available to set"); - return false; - } - - // Apply IK solved arm joints to state - robot_state->setJointGroupPositions(grasp_data_->arm_jmg_, pregrasp_ik_solution_); - - // Set end effector to correct configuration - grasp_data_->setRobotState(robot_state, grasp_.pre_grasp_posture); - - return true; -} - -bool GraspCandidate::getGraspStateOpen(moveit::core::RobotStatePtr& robot_state) -{ - // Error check - if (grasp_ik_solution_.empty()) - { - ROS_ERROR_STREAM_NAMED("grasp_candidate", "No grasp ik solution available to set"); - return false; - } - - // Apply IK solved arm joints to state - robot_state->setJointGroupPositions(grasp_data_->arm_jmg_, grasp_ik_solution_); - - // Set end effector to correct configuration - return getGraspStateOpenEEOnly(robot_state); -} - -bool GraspCandidate::getGraspStateOpenEEOnly(moveit::core::RobotStatePtr& robot_state) -{ - return grasp_data_->setRobotState(robot_state, grasp_.pre_grasp_posture); -} - -bool GraspCandidate::getGraspStateClosed(moveit::core::RobotStatePtr& robot_state) -{ - if (grasp_ik_solution_.empty()) - { - ROS_ERROR_STREAM_NAMED("grasp_candidate", "No grasp ik solution available to set"); - return false; - } - - // Apply IK solved arm joints to state - robot_state->setJointGroupPositions(grasp_data_->arm_jmg_, grasp_ik_solution_); - - // Set end effector to correct configuration - return getGraspStateClosedEEOnly(robot_state); -} - -bool GraspCandidate::getGraspStateClosedEEOnly(moveit::core::RobotStatePtr& robot_state) -{ - return grasp_data_->setRobotState(robot_state, grasp_.grasp_posture); -} - -bool GraspCandidate::isValid() -{ - return grasp_filtered_code_ == GraspFilterCode::NOT_FILTERED; -} - -} // namespace moveit_grasps diff --git a/src/mtc/src/grasp_data.cpp b/src/mtc/src/grasp_data.cpp deleted file mode 100644 index e69114492a8726ce11da01ee908289a331e6c654..0000000000000000000000000000000000000000 --- a/src/mtc/src/grasp_data.cpp +++ /dev/null @@ -1,212 +0,0 @@ -/* - * Software License Agreement (Modified BSD License) - * - * Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Univ of CO, Boulder, PAL Robotics, S.L. - * nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific - * prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/* Authors: Dave Coleman, Bence Magyar - Description: Data class used by the grasp generator. -*/ - -#include "../include/grasp_data.h" - -// Eigen -#include <Eigen/Core> -#include <Eigen/Geometry> -#include <eigen_conversions/eigen_msg.h> - -// C++ -#include <cmath> - -// Parameter loading -#include <rosparam_shortcuts/rosparam_shortcuts.h> - -// Pose conversion -#include <rviz_visual_tools/rviz_visual_tools.h> - -namespace moveit_grasps -{ -const std::string LOGNAME = "grasp_data"; - -GraspData::GraspData(const ros::NodeHandle& nh, const std::string& end_effector, - const moveit::core::RobotModelConstPtr& robot_model) - : base_link_("/base_link"), robot_model_(robot_model) -{ -} - -bool GraspData::loadGraspData(const ros::NodeHandle& nh, const std::string& end_effector) -{ - std::vector<std::string> joint_names; - std::vector<double> pre_grasp_posture; // todo: remove all underscore post-fixes - std::vector<double> grasp_posture; - double pregrasp_time_from_start; - double grasp_time_from_start; - std::string end_effector_name; - - // Helper to let user know what is wrong - if (!nh.hasParam("base_link")) - { - ROS_ERROR_STREAM_NAMED("grasp_data", "Grasp configuration parameter `base_link` missing from rosparam " - "server. Did you load your end effector's configuration yaml file? " - "Searching in namespace: " - << nh.getNamespace()); - return false; - } - - // Load all other parameters - const std::string parent_name = "grasp_data"; // for namespacing logging messages - std::size_t error = 0; - error += !rosparam_shortcuts::get(parent_name, nh, "base_link", base_link_); - - // Search within the sub-namespace of this end effector name - ros::NodeHandle child_nh(nh, end_effector); - - error += !rosparam_shortcuts::get(parent_name, child_nh, "pregrasp_time_from_start", pregrasp_time_from_start); - error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_time_from_start", grasp_time_from_start); - error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_resolution", grasp_resolution_); - error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_min_depth", grasp_min_depth_); - error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_max_depth", grasp_max_depth_); - error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_depth_resolution", grasp_depth_resolution_); - error += !rosparam_shortcuts::get(parent_name, child_nh, "approach_distance_desired", approach_distance_desired_); - error += !rosparam_shortcuts::get(parent_name, child_nh, "retreat_distance_desired", retreat_distance_desired_); - error += !rosparam_shortcuts::get(parent_name, child_nh, "lift_distance_desired", lift_distance_desired_); - error += !rosparam_shortcuts::get(parent_name, child_nh, "angle_resolution", angle_resolution_); - error += !rosparam_shortcuts::get(parent_name, child_nh, "end_effector_name", end_effector_name); - error += !rosparam_shortcuts::get(parent_name, child_nh, "joints", joint_names); - error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_padding_on_approach", grasp_padding_on_approach_); - error += !rosparam_shortcuts::get(parent_name, child_nh, "grasp_posture", grasp_posture); - error += !rosparam_shortcuts::get(parent_name, child_nh, "pregrasp_posture", pre_grasp_posture); - - bool define_tcp_by_name; - child_nh.param<bool>("define_tcp_by_name", define_tcp_by_name, false); - if (define_tcp_by_name) - error += !rosparam_shortcuts::get(parent_name, child_nh, "tcp_name", tcp_name_); - else - error += !rosparam_shortcuts::get(parent_name, child_nh, "tcp_to_eef_mount_transform", tcp_to_eef_mount_); - - rosparam_shortcuts::shutdownIfError(parent_name, error); - - // Create pre-grasp and grasp postures - pre_grasp_posture_.header.frame_id = base_link_; - pre_grasp_posture_.header.stamp = ros::Time::now(); - // Name of joints: - pre_grasp_posture_.joint_names = joint_names; - // Position of joints - pre_grasp_posture_.points.resize(1); - pre_grasp_posture_.points[0].positions = pre_grasp_posture; - pre_grasp_posture_.points[0].time_from_start = ros::Duration(pregrasp_time_from_start); - - // Create grasp posture - grasp_posture_.header.frame_id = base_link_; - grasp_posture_.header.stamp = ros::Time::now(); - // Name of joints: - grasp_posture_.joint_names = joint_names; - // Position of joints - grasp_posture_.points.resize(1); - grasp_posture_.points[0].positions = grasp_posture; - grasp_posture_.points[0].time_from_start = ros::Duration(grasp_time_from_start); - - // Copy values from RobotModel - ee_jmg_ = robot_model_->getJointModelGroup(end_effector_name); - arm_jmg_ = robot_model_->getJointModelGroup(ee_jmg_->getEndEffectorParentGroup().first); - parent_link_ = robot_model_->getLinkModel(ee_jmg_->getEndEffectorParentGroup().second); - - ROS_INFO_NAMED("grasp_data", "ee_name: %s, arm_jmg: %s, parent_link: %s", ee_jmg_->getName().c_str(), - arm_jmg_->getName().c_str(), parent_link_->getName().c_str()); - - if (define_tcp_by_name) - { - robot_state::RobotState state(robot_model_); - state.setToDefaultValues(); - state.update(); - if (!state.knowsFrameTransform(parent_link_->getName())) - { - ROS_ERROR_NAMED("grasp_data", - "Robot Model does not know the frame transform for the end effector group parent " - "frame: %s. Did you set a parent link in the srdf?", - parent_link_->getName().c_str()); - } - if (!state.knowsFrameTransform(tcp_name_)) - { - ROS_ERROR_NAMED("grasp_data", - "Robot Model does not know the frame transform for the tcp frame: %s. Is it " - "available in the urdf?", - tcp_name_.c_str()); - } - Eigen::Isometry3d eef_mount_pose = state.getGlobalLinkTransform(parent_link_); - Eigen::Isometry3d tcp_mount_pose = state.getGlobalLinkTransform(tcp_name_); - tcp_to_eef_mount_ = tcp_mount_pose.inverse() * eef_mount_pose; - } - - return true; -} - -bool GraspData::setRobotStatePreGrasp(robot_state::RobotStatePtr& robot_state) -{ - ROS_WARN_STREAM_NAMED("grasp_data", "setRobotStatePreGrasp is probably wrong"); - return setRobotState(robot_state, pre_grasp_posture_); -} - -bool GraspData::setRobotStateGrasp(robot_state::RobotStatePtr& robot_state) -{ - ROS_WARN_STREAM_NAMED("grasp_data", "setRobotStateGrasp is probably wrong"); - return setRobotState(robot_state, grasp_posture_); -} - -bool GraspData::setRobotState(robot_state::RobotStatePtr& robot_state, const trajectory_msgs::JointTrajectory& posture) -{ - // TODO(davetcoleman): make this more efficient - // Do for every joint in end effector - for (std::size_t i = 0; i < posture.joint_names.size(); ++i) - { - // Set joint position - robot_state->setJointPositions(posture.joint_names[i], posture.points[0].positions); - } - return true; -} - -void GraspData::print() -{ - ROS_WARN_STREAM_NAMED("grasp_data", "Debug Grasp Data variable values:"); - std::cout << "tcp_to_eef_mount_: \n" - << tcp_to_eef_mount_.translation() << "\n" - << tcp_to_eef_mount_.rotation() << std::endl; - std::cout << "pre_grasp_posture_: \n" << pre_grasp_posture_ << std::endl; - std::cout << "grasp_posture_: \n" << grasp_posture_ << std::endl; - std::cout << "base_link_: " << base_link_ << std::endl; - std::cout << "ee_group_: " << ee_jmg_->getName() << std::endl; - std::cout << "angle_resolution_: " << angle_resolution_ << std::endl; - std::cout << "grasp_max_depth_: " << grasp_max_depth_ << std::endl; - std::cout << "grasp_padding_on_approach_: " << grasp_padding_on_approach_ << std::endl; -} - -} // namespace moveit_grasps diff --git a/src/mtc/src/grasp_filter.cpp b/src/mtc/src/grasp_filter.cpp deleted file mode 100644 index fd7ec8f9e4d9fec29605cafcbc4640e5fb1061c2..0000000000000000000000000000000000000000 --- a/src/mtc/src/grasp_filter.cpp +++ /dev/null @@ -1,884 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman <dave@picknik.ai> - Desc: Filters grasps based on kinematic feasibility and collision -*/ - -// moveit_grasps -#include "../include/grasp_filter.h" -#include "../include/state_validity_callback.h" - -// moveit -#include <moveit/transforms/transforms.h> -#include <moveit/collision_detection/collision_tools.h> - -// Conversions -#include <eigen_conversions/eigen_msg.h> - -// Parameter loading -#include <rosparam_shortcuts/rosparam_shortcuts.h> - -namespace moveit_grasps -{ -// Constructor -GraspFilter::GraspFilter(const robot_state::RobotStatePtr& robot_state, - const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools) - : name_("grasp_filter"), visual_tools_(visual_tools), nh_("~/moveit_grasps/filter") -{ - // Make a copy of the robot state so that we are sure outside influence does not break our grasp filter - robot_state_ = std::make_shared<moveit::core::RobotState>(*robot_state); - robot_state_->update(); // make sure transforms are computed - - // Load visulization settings - const std::string parent_name = "grasp_filter"; // for namespacing logging messages - std::size_t error = 0; - // clang-format off - error += !rosparam_shortcuts::get(parent_name, nh_, "collision_verbose", collision_verbose_); - error += !rosparam_shortcuts::get(parent_name, nh_, "statistics_verbose", statistics_verbose_); - error += !rosparam_shortcuts::get(parent_name, nh_, "collision_verbose_speed", collision_verbose_speed_); - error += !rosparam_shortcuts::get(parent_name, nh_, "show_filtered_grasps", show_filtered_grasps_); - error += !rosparam_shortcuts::get(parent_name, nh_, "show_filtered_arm_solutions", show_filtered_arm_solutions_); - error += !rosparam_shortcuts::get(parent_name, nh_, "show_cutting_planes", show_cutting_planes_); - error += !rosparam_shortcuts::get(parent_name, nh_, "show_filtered_arm_solutions_speed", show_filtered_arm_solutions_speed_); - error += !rosparam_shortcuts::get(parent_name, nh_, "show_filtered_arm_solutions_pregrasp_speed", show_filtered_arm_solutions_pregrasp_speed_); - error += !rosparam_shortcuts::get(parent_name, nh_, "show_grasp_filter_collision_if_failed", show_grasp_filter_collision_if_failed_); - // clang-format on - - rosparam_shortcuts::shutdownIfError(parent_name, error); - - // Add planning scene publisher for debugging - planning_scene_publisher_ = nh_.advertise<moveit_msgs::PlanningScene>("/published_planning_scene", 100, false); -} - -bool GraspFilter::filterGrasps(std::vector<GraspCandidatePtr>& grasp_candidates, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const robot_model::JointModelGroup* arm_jmg, - const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, - const std::string& target_object_id) -{ - planning_scene::PlanningScenePtr planning_scene; - { - planning_scene_monitor::LockedPlanningSceneRO scene(planning_scene_monitor); - planning_scene = planning_scene::PlanningScene::clone(scene); - } - return filterGrasps(grasp_candidates, planning_scene, arm_jmg, seed_state, filter_pregrasp, target_object_id); -} - -bool GraspFilter::filterGrasps(std::vector<GraspCandidatePtr>& grasp_candidates, - const planning_scene::PlanningScenePtr& planning_scene, - const robot_model::JointModelGroup* arm_jmg, - const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, - const std::string& target_object_id) -{ - bool verbose = false; - - // Error check - if (grasp_candidates.empty()) - { - ROS_ERROR_NAMED(name_, "Unable to filter grasps because vector is empty"); - return false; - } - if (!filter_pregrasp) - ROS_WARN_STREAM_NAMED(name_, "Not filtering pre-grasp - GraspCandidate may have bad data"); - - // Visualize the cutting planes if desired - visualizeCuttingPlanes(); - - // Get the solver timeout from kinematics.yaml - solver_timeout_ = arm_jmg->getDefaultIKTimeout(); - ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Grasp filter IK timeout " << solver_timeout_); - - // Choose how many degrees of freedom - num_variables_ = arm_jmg->getVariableCount(); - ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Solver for " << num_variables_ << " degrees of freedom"); - - // Get the end effector joint model group - if (arm_jmg->getAttachedEndEffectorNames().size() == 0) - { - ROS_ERROR_STREAM_NAMED(name_, "No end effectors attached to this arm"); - return false; - } - else if (arm_jmg->getAttachedEndEffectorNames().size() > 1) - { - ROS_ERROR_STREAM_NAMED(name_, "More than one end effectors attached to this arm"); - return false; - } - - // Try to filter grasps not in verbose mode - std::size_t remaining_grasps = filterGraspsHelper(grasp_candidates, planning_scene, arm_jmg, seed_state, - filter_pregrasp, verbose, target_object_id); - - // Print stats - printFilterStatistics(grasp_candidates); - - if (remaining_grasps == 0) - { - ROS_INFO_STREAM_NAMED(name_, "Grasp filters removed all grasps"); - if (show_grasp_filter_collision_if_failed_) - { - ROS_INFO_STREAM_NAMED(name_, "Re-running in verbose mode since it failed"); - verbose = true; - remaining_grasps = filterGraspsHelper(grasp_candidates, planning_scene, arm_jmg, seed_state, filter_pregrasp, - verbose, target_object_id); - } - else - ROS_INFO_STREAM_NAMED(name_, "NOT re-running in verbose mode"); - } - - // Visualize valid grasps as arrows with cartesian path as well - if (show_filtered_grasps_) - { - ROS_INFO_STREAM_NAMED(name_, "Showing filtered grasps"); - visualizeGrasps(grasp_candidates, arm_jmg); - } - - // Visualize valid grasp as arm positions - if (show_filtered_arm_solutions_) - { - ROS_INFO_STREAM_NAMED(name_, "Showing filtered arm solutions"); - visualizeCandidateGrasps(grasp_candidates); - } - - if (grasp_candidates.empty()) - { - ROS_WARN_STREAM_NAMED(name_, "No grasps remaining after filtering"); - return false; - } - - return true; -} - -bool GraspFilter::filterGraspByPlane(GraspCandidatePtr& grasp_candidate, const Eigen::Isometry3d& filter_pose, - GraspParallelPlane plane, int direction) const -{ - Eigen::Isometry3d grasp_pose; - Eigen::Vector3d grasp_position; - - // get grasp translation in filter pose CS - grasp_pose = visual_tools_->convertPose(grasp_candidate->grasp_.grasp_pose.pose); - grasp_position = filter_pose.inverse() * grasp_pose.translation(); - - // filter grasps by cutting plane - double epsilon = 0.00000001; - switch (plane) - { - case XY: - if ((direction == -1 && grasp_position(2) < 0 + epsilon) || (direction == 1 && grasp_position(2) > 0 - epsilon)) - grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE; - break; - case XZ: - if ((direction == -1 && grasp_position(1) < 0 + epsilon) || (direction == 1 && grasp_position(1) > 0 - epsilon)) - grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE; - break; - case YZ: - if ((direction == -1 && grasp_position(0) < 0 + epsilon) || (direction == 1 && grasp_position(0) > 0 - epsilon)) - grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE; - break; - default: - ROS_WARN_STREAM_NAMED("filter_by_plane", "plane not specified correctly"); - break; - } - - return grasp_candidate->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE; -} - -bool GraspFilter::filterGraspByOrientation(GraspCandidatePtr& grasp_candidate, const Eigen::Isometry3d& desired_pose, - double max_angular_offset) const -{ - Eigen::Isometry3d tcp_grasp_pose; - Eigen::Isometry3d eef_mount_grasp_pose; - Eigen::Vector3d desired_z_axis; - Eigen::Vector3d grasp_z_axis; - double angle; - - // convert grasp pose back to standard grasping orientation - eef_mount_grasp_pose = visual_tools_->convertPose(grasp_candidate->grasp_.grasp_pose.pose); - tcp_grasp_pose = eef_mount_grasp_pose * grasp_candidate->grasp_data_->tcp_to_eef_mount_.inverse(); - - // compute the angle between the z-axes of the desired and grasp poses - grasp_z_axis = tcp_grasp_pose.rotation() * Eigen::Vector3d(0, 0, 1); - desired_z_axis = desired_pose.rotation() * Eigen::Vector3d(0, 0, 1); - double cos_angle = grasp_z_axis.normalized().dot(desired_z_axis.normalized()); - angle = acos(std::max(-1.0, std::min(1.0, cos_angle))); - - if (angle > max_angular_offset) - { - ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "grasp filtered by orientation"); - grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_ORIENTATION; - return true; - } - else - return false; -} - -bool GraspFilter::filterGraspByGraspIK(const GraspCandidatePtr& grasp_candidate, std::vector<double>& grasp_ik_solution, - const IkThreadStructPtr& ik_thread_struct) const -{ - // Get pose - ik_thread_struct->ik_pose_ = grasp_candidate->grasp_.grasp_pose; - - // Create constraint_fn - moveit::core::GroupStateValidityCallbackFn constraint_fn = - boost::bind(&isGraspStateValid, ik_thread_struct->planning_scene_.get(), - collision_verbose_ || ik_thread_struct->visual_debug_, collision_verbose_speed_, visual_tools_, _1, - _2, _3); - - // Set gripper position (eg. how open the eef is) to the custom open position - grasp_candidate->getGraspStateOpenEEOnly(ik_thread_struct->robot_state_); - - // Solve IK Problem for grasp posture - grasp_ik_solution.resize(0); - if (!findIKSolution(grasp_ik_solution, ik_thread_struct, grasp_candidate, constraint_fn)) - { - ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Unable to find the-grasp IK solution"); - grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_IK; - } - return grasp_candidate->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_IK; -} - -bool GraspFilter::filterGraspByPreGraspIK(const GraspCandidatePtr& grasp_candidate, - std::vector<double>& pregrasp_ik_solution, - const IkThreadStructPtr& ik_thread_struct) const -{ - if (!ik_thread_struct->filter_pregrasp_) - { - ROS_DEBUG_STREAM_NAMED(name_, "Not filtering pregrasp"); - return true; - } - - // Set IK target pose to the pre-grasp pose - const std::string& ee_parent_link_name = grasp_candidate->grasp_data_->ee_jmg_->getEndEffectorParentGroup().second; - ik_thread_struct->ik_pose_ = GraspGenerator::getPreGraspPose(grasp_candidate, ee_parent_link_name); - - moveit::core::GroupStateValidityCallbackFn constraint_fn = - boost::bind(&isGraspStateValid, ik_thread_struct->planning_scene_.get(), - collision_verbose_ || ik_thread_struct->visual_debug_, collision_verbose_speed_, visual_tools_, _1, - _2, _3); - - // Solve IK Problem for pregrasp - pregrasp_ik_solution.resize(0); - if (!findIKSolution(pregrasp_ik_solution, ik_thread_struct, grasp_candidate, constraint_fn)) - { - ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Unable to find PRE-grasp IK solution"); - grasp_candidate->grasp_filtered_code_ = GraspFilterCode::PREGRASP_FILTERED_BY_IK; - } - return grasp_candidate->grasp_filtered_code_ == GraspFilterCode::PREGRASP_FILTERED_BY_IK; -} - -std::size_t -GraspFilter::filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const robot_model::JointModelGroup* arm_jmg, - const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, bool visualize, - const std::string& target_object_id) -{ - planning_scene::PlanningScenePtr planning_scene; - { - planning_scene_monitor::LockedPlanningSceneRO scene(planning_scene_monitor); - planning_scene = planning_scene::PlanningScene::clone(scene); - } - return filterGraspsHelper(grasp_candidates, planning_scene, arm_jmg, seed_state, filter_pregrasp, visualize, - target_object_id); -} - -std::size_t GraspFilter::filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates, - const planning_scene::PlanningScenePtr& planning_scene, - const robot_model::JointModelGroup* arm_jmg, - const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, - bool visualize, const std::string& target_object_id) -{ - // Clone the planning scene for const correctness - planning_scene::PlanningScenePtr planning_scene_clone = planning_scene::PlanningScene::clone(planning_scene); - - // Setup collision checking - *robot_state_ = planning_scene_clone->getCurrentState(); - - // Check that we have grasp candidates - if (!grasp_candidates.size()) - { - ROS_WARN_NAMED(name_, "filterGraspsHelper passed empty grasp candidates"); - return 0; - } - - // Choose Number of cores - std::size_t num_threads = omp_get_max_threads(); - if (num_threads > grasp_candidates.size()) - { - num_threads = grasp_candidates.size(); - } - - // Debug - if (visualize || collision_verbose_) - { - num_threads = 1; - ROS_WARN_STREAM_NAMED(name_, "Using only " << num_threads << " threads because verbose is true"); - } - ROS_INFO_STREAM_NAMED(name_, "Filtering " << grasp_candidates.size() << " candidate grasps with " << num_threads - << " threads"); - - // Load kinematic solvers if not already loaded - if (kin_solvers_[arm_jmg->getName()].size() != num_threads) - { - kin_solvers_[arm_jmg->getName()].clear(); - - // Create an ik solver for every thread - for (std::size_t i = 0; i < num_threads; ++i) - { - // ROS_DEBUG_STREAM_NAMED(name_,"Creating ik solver " << i); - kin_solvers_[arm_jmg->getName()].push_back(arm_jmg->getSolverInstance()); - - // Test to make sure we have a valid kinematics solver - if (!kin_solvers_[arm_jmg->getName()][i]) - { - ROS_ERROR_STREAM_NAMED(name_, "No kinematic solver found"); - return 0; - } - } - } - - // Robot states - // Create a robot state for every thread - if (robot_states_.size() != num_threads) - { - robot_states_.clear(); - for (std::size_t i = 0; i < num_threads; ++i) - { - // Copy the previous robot state - robot_states_.push_back(std::make_shared<robot_state::RobotState>(*robot_state_)); - } - } - else // update the states - { - for (std::size_t i = 0; i < num_threads; ++i) - { - // Copy the previous robot state - *(robot_states_[i]) = *robot_state_; - } - } - - // Transform poses - // bring the pose to the frame of the IK solver - const std::string& ik_frame = kin_solvers_[arm_jmg->getName()][0]->getBaseFrame(); - Eigen::Isometry3d link_transform; - ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", - "Frame transform from ik_frame: " << ik_frame << " and robot model frame: " - << robot_state_->getRobotModel()->getModelFrame()); - if (!moveit::core::Transforms::sameFrame(ik_frame, robot_state_->getRobotModel()->getModelFrame())) - { - const robot_model::LinkModel* lm = - robot_state_->getLinkModel((!ik_frame.empty() && ik_frame[0] == '/') ? ik_frame.substr(1) : ik_frame); - - if (!lm) - { - ROS_ERROR_STREAM_NAMED(name_, "Unable to find frame for link transform"); - return 0; - } - - link_transform = robot_state_->getGlobalLinkTransform(lm).inverse(); - } - - // Ensure the ACM entries are set to ignore collisions between the eef and the target object - if (!target_object_id.empty()) - { - if (!planning_scene_clone->knowsFrameTransform(target_object_id)) - { - ROS_ERROR_STREAM_NAMED(name_, "target_object_id: " << target_object_id << " unknown to the planning scene"); - return 0; - } - std::vector<std::string> ee_links = grasp_candidates.front()->grasp_data_->ee_jmg_->getLinkModelNames(); - if (!ee_links.empty()) - { - setACMFingerEntry(target_object_id, true, ee_links, planning_scene_clone); - } - } - - // Create the seed state vector - std::vector<double> ik_seed_state; - seed_state->copyJointGroupPositions(arm_jmg, ik_seed_state); - - // Thread data - // Allocate only once to increase performance - std::vector<IkThreadStructPtr> ik_thread_structs; - ik_thread_structs.resize(num_threads); - for (std::size_t thread_id = 0; thread_id < num_threads; ++thread_id) - { - ik_thread_structs[thread_id] = - std::make_shared<IkThreadStruct>(grasp_candidates, planning_scene_clone, link_transform, - 0, // this is filled in by OpenMP - kin_solvers_[arm_jmg->getName()][thread_id], robot_states_[thread_id], - solver_timeout_, filter_pregrasp, visualize, thread_id, target_object_id); - ik_thread_structs[thread_id]->ik_seed_state_ = ik_seed_state; - } - - // Benchmark time - ros::Time start_time; - start_time = ros::Time::now(); - - // Loop through poses and find those that are kinematically feasible - - omp_set_num_threads(num_threads); -#pragma omp parallel for schedule(dynamic) - for (std::size_t grasp_id = 0; grasp_id < grasp_candidates.size(); ++grasp_id) - { - std::size_t thread_id = omp_get_thread_num(); - ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Thread " << thread_id << " processing grasp " << grasp_id); - - // If in verbose mode allow for quick exit - if (ik_thread_structs[thread_id]->visual_debug_ && !ros::ok()) - continue; // breaking a for loop is not allows with OpenMP - - // Assign grasp to process - ik_thread_structs[thread_id]->grasp_id = grasp_id; - - // Process the grasp if it hasn't already been filtered out - if (grasp_candidates[grasp_id]->isValid()) - processCandidateGrasp(ik_thread_structs[thread_id]); - } - - if (statistics_verbose_) - { - // End Benchmark time - double duration = (ros::Time::now() - start_time).toSec(); - ROS_INFO_STREAM_NAMED(name_ + ".print_filter_statistics", "==================================================="); - ROS_INFO_STREAM_NAMED(name_ + ".print_filter_statistics", "FILTER DURATION"); - ROS_INFO_STREAM_NAMED(name_ + ".print_filter_statistics", "Grasp Filter Duration :\t" << duration); - ROS_INFO_STREAM_NAMED(name_ + ".print_filter_statistics", "---------------------------------------------------"); - } - - std::size_t not_filtered = 0; - for (std::size_t i = 0; i < grasp_candidates.size(); ++i) - if (grasp_candidates[i]->isValid()) - ++not_filtered; - - return not_filtered; -} - -void GraspFilter::printFilterStatistics(const std::vector<GraspCandidatePtr>& grasp_candidates) const -{ - if (!statistics_verbose_) - return; - static const std::string logger_name = name_ + ".filter_statistics"; - // Count number of grasps remaining - std::size_t not_filtered = 0; - std::size_t grasp_filtered_by_ik = 0; - std::size_t grasp_filtered_by_cutting_plane = 0; - std::size_t grasp_filtered_by_orientation = 0; - std::size_t pregrasp_filtered_by_ik = 0; - - for (std::size_t i = 0; i < grasp_candidates.size(); ++i) - { - if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_IK) - ++grasp_filtered_by_ik; - else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE) - ++grasp_filtered_by_cutting_plane; - else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_ORIENTATION) - ++grasp_filtered_by_orientation; - else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::PREGRASP_FILTERED_BY_IK) - ++pregrasp_filtered_by_ik; - else if (grasp_candidates[i]->isValid()) - ++not_filtered; - } - - ROS_INFO_STREAM_NAMED(logger_name, "======================================================="); - ROS_INFO_STREAM_NAMED(logger_name, "GRASP FILTER RESULTS "); - ROS_INFO_STREAM_NAMED(logger_name, "Total candidate grasps " << grasp_candidates.size()); - ROS_INFO_STREAM_NAMED(logger_name, "Total valid grasps " << not_filtered); - ROS_INFO_STREAM_NAMED(logger_name, "-------------------------------------------------------"); - ROS_INFO_STREAM_NAMED(logger_name, "grasp_filtered_by_cutting_plane " << grasp_filtered_by_cutting_plane); - ROS_INFO_STREAM_NAMED(logger_name, "grasp_filtered_by_orientation " << grasp_filtered_by_orientation); - ROS_INFO_STREAM_NAMED(logger_name, "grasp_filtered_by_ik " << grasp_filtered_by_ik); - ROS_INFO_STREAM_NAMED(logger_name, "pregrasp_filtered_by_ik " << pregrasp_filtered_by_ik); -} - -bool GraspFilter::processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct) -{ - ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "Checking grasp #" << ik_thread_struct->grasp_id); - - // Helper pointer - GraspCandidatePtr& grasp_candidate = ik_thread_struct->grasp_candidates_[ik_thread_struct->grasp_id]; - - // Get pose - ik_thread_struct->ik_pose_ = grasp_candidate->grasp_.grasp_pose; - - // Filter by cutting planes - for (auto& cutting_plane : cutting_planes_) - { - if (filterGraspByPlane(grasp_candidate, cutting_plane->pose_, cutting_plane->plane_, cutting_plane->direction_)) - { - return false; - } - } - - // Filter by desired orientation - for (auto& desired_grasp_orientation : desired_grasp_orientations_) - { - if (filterGraspByOrientation(grasp_candidate, desired_grasp_orientation->pose_, - desired_grasp_orientation->max_angle_offset_)) - { - return false; - } - } - - std::vector<double> grasp_ik_solution; - if (filterGraspByGraspIK(grasp_candidate, grasp_ik_solution, ik_thread_struct)) - { - return false; - } - - // Assign the grasp_ik solution. - // TODO: This should be moved out of filtering - grasp_candidate->grasp_ik_solution_ = grasp_ik_solution; - - // Copy solution to seed state so that next solution is faster - ik_thread_struct->ik_seed_state_ = grasp_ik_solution; - - std::vector<double> pregrasp_ik_solution; - if (filterGraspByPreGraspIK(grasp_candidate, pregrasp_ik_solution, ik_thread_struct)) - { - return false; - } - // Assign the pregrasp_ik solution. - // TODO: This should be moved out of filtering - grasp_candidate->pregrasp_ik_solution_ = pregrasp_ik_solution; - - return true; -} - -bool GraspFilter::findIKSolution(std::vector<double>& ik_solution, const IkThreadStructPtr& ik_thread_struct, - const GraspCandidatePtr& grasp_candidate, - const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const -{ - robot_state::RobotState state = ik_thread_struct->planning_scene_->getCurrentState(); - if (ik_thread_struct->ik_seed_state_.size() == grasp_candidate->grasp_data_->arm_jmg_->getActiveJointModels().size()) - { - state.setJointGroupPositions(grasp_candidate->grasp_data_->arm_jmg_, ik_thread_struct->ik_seed_state_); - state.update(); - } - - // Copy in attached bodies which get removed by setJointGroupPositions for some reason - std::vector<const robot_state::AttachedBody*> attached_bodies; - ik_thread_struct->planning_scene_->getCurrentState().getAttachedBodies(attached_bodies); - - for (const robot_state::AttachedBody* ab : attached_bodies) - state.attachBody(ab->getName(), ab->getPose(), ab->getShapes(), ab->getShapePoses(), ab->getTouchLinks(), - ab->getAttachedLinkName(), ab->getDetachPosture(), ab->getSubframes()); - - bool ik_success = state.setFromIK(grasp_candidate->grasp_data_->arm_jmg_, ik_thread_struct->ik_pose_.pose, - ik_thread_struct->timeout_, constraint_fn); - - // Results - if (ik_success) - { - state.copyJointGroupPositions(grasp_candidate->grasp_data_->arm_jmg_, ik_solution); - return true; - } - else - { - // The grasp was valid but the pre-grasp was not - ROS_DEBUG_STREAM_NAMED(name_ + ".superdebug", "IK solution not found"); - return false; - } -} - -void GraspFilter::addCuttingPlane(const Eigen::Isometry3d& pose, GraspParallelPlane plane, int direction) -{ - cutting_planes_.push_back(std::make_shared<CuttingPlane>(pose, plane, direction)); -} - -void GraspFilter::addDesiredGraspOrientation(const Eigen::Isometry3d& pose, double max_angle_offset) -{ - desired_grasp_orientations_.push_back(std::make_shared<DesiredGraspOrientation>(pose, max_angle_offset)); -} - -bool GraspFilter::removeInvalidAndFilter(std::vector<GraspCandidatePtr>& grasp_candidates) const -{ - std::size_t original_num_grasps = grasp_candidates.size(); - - // Remove all invalid grasps - for (std::vector<GraspCandidatePtr>::iterator it = grasp_candidates.begin(); it != grasp_candidates.end();) - { - // Check if valid grasp - if (!(*it)->isValid()) - { - it = grasp_candidates.erase(it); // not valid - } - else - { - ++it; // was valid, keep - } - } - ROS_INFO_STREAM_NAMED(name_, "Removed " << original_num_grasps - grasp_candidates.size() - << " invalid grasp candidates, " << grasp_candidates.size() << " remaining"); - - // Error Check - if (grasp_candidates.empty()) - { - ROS_WARN_STREAM_NAMED(name_, "No remaining grasp candidates"); - return false; - } - - // Order remaining valid grasps by best score - std::sort(grasp_candidates.begin(), grasp_candidates.end(), compareGraspScores); - - ROS_INFO_STREAM_NAMED(name_, "Sorted valid grasps, highest quality is " - << grasp_candidates.front()->grasp_.grasp_quality << " and lowest quality is " - << grasp_candidates.back()->grasp_.grasp_quality); - - return true; -} - -bool GraspFilter::visualizeGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates, - const moveit::core::JointModelGroup* arm_jmg) -{ - // Publish in batch - visual_tools_->enableBatchPublishing(true); - - /* - NOTE: duplicated in README.md - MAGENTA - grasp filtered by cutting plane - YELLOW - grasp filtered by orientation - RED - grasp filtered by ik - PINK - grasp filtered by collision - BLUE - pregrasp filtered by ik - CYAN - pregrasp filtered by collision - GREY - misc filtered - GREEN - valid - */ - ROS_INFO_STREAM_NAMED(name_, "Showing " << grasp_candidates.size() << " solutions at a speed of " - << show_filtered_arm_solutions_speed_ << "sec per solution"); - ROS_INFO_STREAM_NAMED(name_, "---------------------------------------------"); - ROS_INFO_STREAM_NAMED(name_, " MAGENTA - grasp filtered by cutting plane"); - ROS_INFO_STREAM_NAMED(name_, " YELLOW - grasp filtered by orientation"); - ROS_INFO_STREAM_NAMED(name_, " RED - grasp filtered by ik"); - ROS_INFO_STREAM_NAMED(name_, " PINK - grasp filtered by collision"); - ROS_INFO_STREAM_NAMED(name_, " BLUE - pregrasp filtered by ik"); - ROS_INFO_STREAM_NAMED(name_, " CYAN - pregrasp filtered by collision"); - ROS_INFO_STREAM_NAMED(name_, " GREY - misc filtered"); - ROS_INFO_STREAM_NAMED(name_, " GREEN - valid"); - ROS_INFO_STREAM_NAMED(name_, "---------------------------------------------"); - for (std::size_t i = 0; i < grasp_candidates.size(); ++i) - { - double size = 0.03; // 0.01 * grasp_candidates[i]->grasp_.grasp_quality; - - if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_IK) - { - visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::RED, - rviz_visual_tools::SMALL, size); - } - else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::PREGRASP_FILTERED_BY_IK) - { - visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::BLUE, - rviz_visual_tools::SMALL, size); - } - else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE) - { - visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::MAGENTA, - rviz_visual_tools::SMALL, size); - } - else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::GRASP_FILTERED_BY_ORIENTATION) - { - visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::YELLOW, - rviz_visual_tools::SMALL, size); - } - else if (grasp_candidates[i]->grasp_filtered_code_ == GraspFilterCode::NOT_FILTERED) - { - visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::GREEN, - rviz_visual_tools::SMALL, size); - } - else - visual_tools_->publishZArrow(grasp_candidates[i]->grasp_.grasp_pose.pose, rviz_visual_tools::GREY, - rviz_visual_tools::SMALL, size); - - // Publish in batch - visual_tools_->trigger(); - ros::Duration(.01).sleep(); - } - - return true; -} - -bool GraspFilter::visualizeIKSolutions(const std::vector<GraspCandidatePtr>& grasp_candidates, - const moveit::core::JointModelGroup* arm_jmg, double animation_speed) -{ - // Convert the grasp_candidates into a format moveit_visual_tools can use - std::vector<trajectory_msgs::JointTrajectoryPoint> ik_solutions; - for (std::size_t i = 0; i < grasp_candidates.size(); ++i) - { - // Check if valid grasp - if (!grasp_candidates[i]->isValid()) - continue; - - trajectory_msgs::JointTrajectoryPoint new_point; - new_point.positions = grasp_candidates[i]->grasp_ik_solution_; - ik_solutions.push_back(new_point); - } - - if (ik_solutions.empty()) - { - ROS_ERROR_STREAM_NAMED(name_, "Unable to visualize IK solutions because there are no valid ones"); - return false; - } - - return visual_tools_->publishIKSolutions(ik_solutions, arm_jmg, animation_speed); -} - -bool GraspFilter::visualizeCandidateGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates) -{ - for (std::size_t i = 0; i < grasp_candidates.size(); ++i) - { - // Check if valid grasp - if (!grasp_candidates[i]->isValid()) - continue; - - // Apply the pregrasp state - if (grasp_candidates[i]->getPreGraspState(robot_state_)) - { - // Show in Rviz - visual_tools_->publishRobotState(robot_state_, rviz_visual_tools::ORANGE); - visual_tools_->trigger(); - ros::Duration(show_filtered_arm_solutions_pregrasp_speed_).sleep(); - } - - // Apply the grasp state - if (grasp_candidates[i]->getGraspStateClosed(robot_state_)) - { - // Show in Rviz - visual_tools_->publishRobotState(robot_state_, rviz_visual_tools::WHITE); - visual_tools_->trigger(); - ros::Duration(show_filtered_arm_solutions_speed_).sleep(); - } - } - - return true; -} - -bool GraspFilter::visualizeCuttingPlanes() -{ - if (show_cutting_planes_) - { - for (auto& cutting_plane : cutting_planes_) - { - switch (cutting_plane->plane_) - { - case XY: - visual_tools_->publishXYPlane(cutting_plane->pose_); - break; - case XZ: - visual_tools_->publishXZPlane(cutting_plane->pose_); - break; - case YZ: - visual_tools_->publishYZPlane(cutting_plane->pose_); - break; - default: - ROS_ERROR_STREAM_NAMED(name_, "Unknown cutting plane type"); - } - } - visual_tools_->trigger(); - } - return true; -} - -void GraspFilter::clearCuttingPlanes() -{ - cutting_planes_.clear(); -} - -void GraspFilter::clearDesiredGraspOrientations() -{ - desired_grasp_orientations_.clear(); -} - -bool GraspFilter::addCuttingPlanesForBin(const Eigen::Isometry3d& world_to_bin, const Eigen::Isometry3d& bin_to_product, - const double& bin_width, const double& bin_height) -{ - // Add grasp filters - clearCuttingPlanes(); - clearDesiredGraspOrientations(); - - // Bottom of bin - addCuttingPlane(world_to_bin, XY, -1); - - // Right wall of bin - addCuttingPlane(world_to_bin, XZ, -1); - - // Top of bin - Eigen::Isometry3d world_to_bin_top = world_to_bin; - world_to_bin_top.translation() += Eigen::Vector3d(0, bin_width, bin_height); - addCuttingPlane(world_to_bin_top, XY, 1); - - // Left wall of bin - addCuttingPlane(world_to_bin_top, XZ, 1); - - // // Back half of product - Eigen::Isometry3d world_to_bin_back = world_to_bin; - world_to_bin_back.translation() += - Eigen::Vector3d(bin_to_product.translation().x(), bin_width / 2.0, bin_height / 2.0); - addCuttingPlane(world_to_bin_back, YZ, 1); - - return true; -} - -void GraspFilter::setACMFingerEntry(const std::string& object_name, bool allowed, - const std::vector<std::string>& ee_link_names, - const planning_scene::PlanningScenePtr& scene) -{ - static const std::string logger_name = name_ + ".set_acm_finger_entry"; - ROS_DEBUG_STREAM_NAMED(logger_name, "" << object_name.c_str() << ", " << (allowed ? "true" : "false")); - - // Lock planning scene - for (std::size_t i = 0; i < ee_link_names.size(); ++i) - { - ROS_DEBUG_NAMED(logger_name, "collisions between %s and %s : %s", object_name.c_str(), ee_link_names[i].c_str(), - allowed ? "allowed" : "not allowed"); - scene->getAllowedCollisionMatrixNonConst().setEntry(object_name, ee_link_names[i], allowed); - } - - // Debug current matrix - if (false) - { - moveit_msgs::AllowedCollisionMatrix msg; - scene->getAllowedCollisionMatrix().getMessage(msg); - ROS_DEBUG_STREAM_NAMED(logger_name, "Current collision matrix: " << msg); - } -} - -void GraspFilter::publishPlanningScene(const planning_scene::PlanningScenePtr& ps) const -{ - moveit_msgs::PlanningScene msg; - ps->getPlanningSceneMsg(msg); - planning_scene_publisher_.publish(msg); -} - -} // namespace moveit_grasps diff --git a/src/mtc/src/grasp_generator.cpp b/src/mtc/src/grasp_generator.cpp deleted file mode 100644 index 95b469ba40a6db936dd90b72708e79fe7b13dabc..0000000000000000000000000000000000000000 --- a/src/mtc/src/grasp_generator.cpp +++ /dev/null @@ -1,199 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy - Desc: Generates geometric grasps for cuboids and blocks, not using physics or contact wrenches -*/ - -#include "../include/grasp_generator.h" -#include "../include/grasp_filter.h" - -#include <rosparam_shortcuts/rosparam_shortcuts.h> - -namespace -{ -void debugFailedOpenGripper(double percent_open, double min_finger_open_on_approach, double object_width, - double grasp_padding_on_approach) -{ - ROS_ERROR_STREAM_NAMED("grasp_generator", "Unable to set grasp width to " - << percent_open << " % open. Stats:" - << "\n min_finger_open_on_approach: \t " << min_finger_open_on_approach - << "\n object_width: \t " << object_width - << "\n grasp_padding_on_approach_: \t " << grasp_padding_on_approach); -} - -} // namespace - -namespace moveit_grasps - -{ -// Constructor -GraspGenerator::GraspGenerator(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, bool verbose) - : ideal_grasp_pose_(Eigen::Isometry3d::Identity()) - , visual_tools_(visual_tools) - , verbose_(verbose) - , nh_("~/moveit_grasps/generator") - , grasp_score_weights_(std::make_shared<GraspScoreWeights>()) -{ - // Load visulization settings - const std::string parent_name = "grasps"; // for namespacing logging messages - std::size_t error = 0; - - error += !rosparam_shortcuts::get(parent_name, nh_, "verbose", verbose_); - error += !rosparam_shortcuts::get(parent_name, nh_, "show_prefiltered_grasps", show_prefiltered_grasps_); - error += !rosparam_shortcuts::get(parent_name, nh_, "show_prefiltered_grasps_speed", show_prefiltered_grasps_speed_); - - // Load scoring weights - rosparam_shortcuts::shutdownIfError(parent_name, error); -} - -void GraspGenerator::setIdealTCPGraspPoseRPY(const std::vector<double>& ideal_grasp_orientation_rpy) -{ - ROS_ASSERT_MSG(ideal_grasp_orientation_rpy.size() == 3, "setIdealTCPGraspPoseRPY must be set with a vector of length " - "3"); - - // copy the ideal_grasp_pose.translation() so that we only change the orientation. - Eigen::Vector3d ideal_grasp_pose_translation(ideal_grasp_pose_.translation()); - - // Set ideal grasp pose (currently only uses orientation of pose) - ideal_grasp_pose_ = Eigen::Isometry3d::Identity() * - Eigen::AngleAxisd(ideal_grasp_orientation_rpy[0], Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(ideal_grasp_orientation_rpy[1], Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(ideal_grasp_orientation_rpy[2], Eigen::Vector3d::UnitZ()); - - ideal_grasp_pose_.translation() = ideal_grasp_pose_translation; -} - -Eigen::Vector3d GraspGenerator::getPreGraspDirection(const moveit_msgs::Grasp& grasp, const std::string& ee_parent_link) -{ - // Grasp Pose Variables - Eigen::Isometry3d grasp_pose_eef_mount_eigen; - tf::poseMsgToEigen(grasp.grasp_pose.pose, grasp_pose_eef_mount_eigen); - - // The direction of the pre-grasp in the frame of the parent link - Eigen::Vector3d pre_grasp_approach_direction = - Eigen::Vector3d(grasp.pre_grasp_approach.direction.vector.x, grasp.pre_grasp_approach.direction.vector.y, - grasp.pre_grasp_approach.direction.vector.z); - - // Decide if we need to change the approach_direction to the local frame of the end effector orientation - if (grasp.pre_grasp_approach.direction.header.frame_id == ee_parent_link) - { - // Apply/compute the approach_direction vector in the local frame of the grasp_pose orientation - return grasp_pose_eef_mount_eigen.rotation() * pre_grasp_approach_direction; - } - return pre_grasp_approach_direction; -} - -geometry_msgs::PoseStamped GraspGenerator::getPreGraspPose(const GraspCandidatePtr& grasp_candidate, - const std::string& ee_parent_link) -{ - // Grasp Pose Variables - Eigen::Isometry3d grasp_pose_eef_mount_eigen; - tf::poseMsgToEigen(grasp_candidate->grasp_.grasp_pose.pose, grasp_pose_eef_mount_eigen); - - // Get pre-grasp pose first - Eigen::Isometry3d pre_grasp_pose_eef_mount_eigen = - grasp_pose_eef_mount_eigen; // Copy original grasp pose to pre-grasp pose - - // Approach direction - Eigen::Vector3d pre_grasp_approach_direction_local = getPreGraspDirection(grasp_candidate->grasp_, ee_parent_link); - - // Update the grasp matrix usign the new locally-framed approach_direction - pre_grasp_pose_eef_mount_eigen.translation() -= - pre_grasp_approach_direction_local * grasp_candidate->grasp_.pre_grasp_approach.desired_distance; - - // Convert eigen pre-grasp position back to regular message - geometry_msgs::PoseStamped pre_grasp_pose_eef_mount_msg; - tf::poseEigenToMsg(pre_grasp_pose_eef_mount_eigen, pre_grasp_pose_eef_mount_msg.pose); - - // Copy original header to new grasp - pre_grasp_pose_eef_mount_msg.header = grasp_candidate->grasp_.grasp_pose.header; - - return pre_grasp_pose_eef_mount_msg; -} - -void GraspGenerator::getGraspWaypoints(const GraspCandidatePtr& grasp_candidate, - EigenSTL::vector_Isometry3d& grasp_waypoints) -{ - Eigen::Isometry3d grasp_pose; - tf::poseMsgToEigen(grasp_candidate->grasp_.grasp_pose.pose, grasp_pose); - - const geometry_msgs::PoseStamped pregrasp_pose_msg = - GraspGenerator::getPreGraspPose(grasp_candidate, grasp_candidate->grasp_data_->parent_link_->getName()); - - // Create waypoints - Eigen::Isometry3d pregrasp_pose; - tf::poseMsgToEigen(pregrasp_pose_msg.pose, pregrasp_pose); - - Eigen::Isometry3d lifted_grasp_pose = grasp_pose; - lifted_grasp_pose.translation().z() += grasp_candidate->grasp_data_->lift_distance_desired_; - - // Solve for post grasp retreat - Eigen::Isometry3d retreat_pose = lifted_grasp_pose; - Eigen::Vector3d postgrasp_vector(grasp_candidate->grasp_.post_grasp_retreat.direction.vector.x, - grasp_candidate->grasp_.post_grasp_retreat.direction.vector.y, - grasp_candidate->grasp_.post_grasp_retreat.direction.vector.z); - postgrasp_vector.normalize(); - - retreat_pose.translation() += - retreat_pose.rotation() * postgrasp_vector * grasp_candidate->grasp_.post_grasp_retreat.desired_distance; - - grasp_waypoints.clear(); - grasp_waypoints.resize(4); - grasp_waypoints[0] = pregrasp_pose; - grasp_waypoints[1] = grasp_pose; - grasp_waypoints[2] = lifted_grasp_pose; - grasp_waypoints[3] = retreat_pose; -} - -void GraspGenerator::publishGraspArrow(const geometry_msgs::Pose& grasp, const GraspDataPtr& grasp_data, - const rviz_visual_tools::colors& color, double approach_length) -{ - visual_tools_->publishArrow(grasp, color, rviz_visual_tools::MEDIUM); -} - -bool GraspGenerator::visualizeAnimatedGrasps(const std::vector<GraspCandidatePtr>& grasp_candidates, - const moveit::core::JointModelGroup* ee_jmg, double animation_speed) -{ - // Convert the grasp_candidates into a format moveit_visual_tools can use - std::vector<moveit_msgs::Grasp> grasps; - for (std::size_t i = 0; i < grasp_candidates.size(); ++i) - { - grasps.push_back(grasp_candidates[i]->grasp_); - } - - return visual_tools_->publishAnimatedGrasps(grasps, ee_jmg, show_prefiltered_grasps_speed_); -} - -} // namespace moveit_grasps diff --git a/src/mtc/src/grasp_pipeline.cpp b/src/mtc/src/grasp_pipeline.cpp deleted file mode 100644 index 123b0ff3d7dac4eedc905efb160a6fcda8ddb4fe..0000000000000000000000000000000000000000 --- a/src/mtc/src/grasp_pipeline.cpp +++ /dev/null @@ -1,615 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, PickNik LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - ******************************/ - -// ROS -#include <ros/ros.h> - -// MoveIt -#include <moveit/robot_state/robot_state.h> -#include <moveit/planning_scene_monitor/planning_scene_monitor.h> -#include <moveit/kinematic_constraints/utils.h> -#include <moveit/planning_interface/planning_interface.h> -#include <moveit/planning_pipeline/planning_pipeline.h> -#include <moveit/robot_state/conversions.h> - -// Visualization -#include <moveit_visual_tools/moveit_visual_tools.h> - -// Grasp -#include "../include/two_finger_grasp_generator.h" -#include "../include/two_finger_grasp_data.h" -#include "../include/two_finger_grasp_filter.h" -#include "../include/grasp_planner.h" - - -#include <fstream> -#include "yaml-cpp/yaml.h" -#include "tf2_eigen/tf2_eigen.h" - - -// Parameter loading -#include <rosparam_shortcuts/rosparam_shortcuts.h> - - -YAML::Node node; - -namespace moveit_grasps -{ -static const std::string LOGNAME = "grasp_pipeline_demo"; - -namespace -{ -bool isStateValid(const planning_scene::PlanningScene* planning_scene, - const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, robot_state::RobotState* robot_state, - const robot_model::JointModelGroup* group, const double* ik_solution) -{ - robot_state->setJointGroupPositions(group, ik_solution); - robot_state->update(); - return !planning_scene->isStateColliding(*robot_state, group->getName()); -} - -void waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt) -{ - visual_tools->prompt(prompt); -} - -} // namespace - -class GraspPipeline -{ -public: - // Constructor - GraspPipeline() : nh_("~") - { - // Get arm info from param server - const std::string parent_name = "grasp_filter_demo"; // for namespacing logging messages - rosparam_shortcuts::get(parent_name, nh_, "planning_group_name", planning_group_name_); - rosparam_shortcuts::get(parent_name, nh_, "ee_group_name", ee_group_name_); - - ROS_INFO_STREAM_NAMED("test", "End Effector: " << ee_group_name_); - ROS_INFO_STREAM_NAMED("test", "Planning Group: " << planning_group_name_); - - loadScene(); - setupGraspPipeline(); - } - - void loadScene() - { - // --------------------------------------------------------------------------------------------- - // Load planning scene to share - planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description"); - if (!planning_scene_monitor_->getPlanningScene()) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Planning scene not configured"); - return; - } - planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, - "grasping_planning_scene"); - planning_scene_monitor_->getPlanningScene()->setName("grasping_planning_scene"); - - robot_model_loader::RobotModelLoaderPtr robot_model_loader; - robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>("robot_description"); - - // Load the robot model - robot_model_ = robot_model_loader->getModel(); - arm_jmg_ = robot_model_->getJointModelGroup(planning_group_name_); - - // --------------------------------------------------------------------------------------------- - // Load the Robot Viz Tools for publishing to Rviz - visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>( - robot_model_->getModelFrame(), "/rviz_visual_tools", planning_scene_monitor_); - visual_tools_->loadMarkerPub(); - visual_tools_->loadRobotStatePub("/display_robot_state"); - visual_tools_->loadTrajectoryPub("/display_planned_path"); - visual_tools_->loadSharedRobotState(); - visual_tools_->enableBatchPublishing(); - visual_tools_->deleteAllMarkers(); - visual_tools_->removeAllCollisionObjects(); - visual_tools_->hideRobot(); - visual_tools_->trigger(); - - // Publish the global frame - visual_tools_->publishAxis(Eigen::Isometry3d::Identity()); - visual_tools_->trigger(); - } - - void setupGraspPipeline() - { - // --------------------------------------------------------------------------------------------- - // Load grasp data specific to our robot - grasp_data_ = - std::make_shared<moveit_grasps::TwoFingerGraspData>(nh_, ee_group_name_, visual_tools_->getRobotModel()); - if (!grasp_data_->loadGraspData(nh_, ee_group_name_)) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Failed to load Grasp Data parameters."); - exit(-1); - } - - // --------------------------------------------------------------------------------------------- - // Load grasp generator - grasp_generator_ = std::make_shared<moveit_grasps::TwoFingerGraspGenerator>(visual_tools_); - - // Set the ideal grasp orientation for scoring - std::vector<double> ideal_grasp_rpy = { 3.14, 0.0, 0.0 }; - grasp_generator_->setIdealTCPGraspPoseRPY(ideal_grasp_rpy); - - // Set custom grasp score weights - auto grasp_score_weights = std::make_shared<moveit_grasps::TwoFingerGraspScoreWeights>(); - grasp_score_weights->orientation_x_score_weight_ = 2.0; - grasp_score_weights->orientation_y_score_weight_ = 2.0; - grasp_score_weights->orientation_z_score_weight_ = 2.0; - grasp_score_weights->translation_x_score_weight_ = 1.0; - grasp_score_weights->translation_y_score_weight_ = 1.0; - grasp_score_weights->translation_z_score_weight_ = 1.0; - // Finger gripper specific weights. - grasp_score_weights->depth_score_weight_ = 2.0; - grasp_score_weights->width_score_weight_ = 2.0; - // Assign the grasp score weights in the grasp_generator - grasp_generator_->setGraspScoreWeights(grasp_score_weights); - - // --------------------------------------------------------------------------------------------- - // Load grasp filter - grasp_filter_ = - std::make_shared<moveit_grasps::TwoFingerGraspFilter>(visual_tools_->getSharedRobotState(), visual_tools_); - - // --------------------------------------------------------------------------------------------- - // Load grasp planner for approach, lift and retreat planning - grasp_planner_ = std::make_shared<moveit_grasps::GraspPlanner>(visual_tools_); - - // MoveIt Grasps allows for a manual breakpoint debugging tool to be optionally passed in - grasp_planner_->setWaitForNextStepCallback(boost::bind(&waitForNextStep, visual_tools_, _1)); - - // ----------------------------------------------------- - // Load the motion planning pipeline - planning_pipeline_ = - std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, nh_, "planning_plugin", "request_adapter"); - } - - - - // --------------------------------------------s - // Filtering grasps - // Note: This step also solves for the grasp and pre-grasp states and stores them in gras - /* - void visualizePick(const moveit_grasps::GraspCandidatePtr& valid_grasp_candidate, - const moveit_msgs::MotionPlanResponse& pre_approach_plan) - { - EigenSTL::vector_Isometry3d waypoints; - moveit_grasps::GraspGenerator::getGraspWaypoints(valid_grasp_candidate, waypoints); - - // Visualize waypoints - visual_tools_->publishAxisLabeled(waypoints[0], "pregrasp"); - visual_tools_->publishAxisLabeled(waypoints[1], "grasp"); - visual_tools_->publishAxisLabeled(waypoints[2], "lifted"); - visual_tools_->publishAxisLabeled(waypoints[3], "retreat"); - visual_tools_->trigger(); - - // Get the pre and post grasp states - visual_tools_->prompt("pre_grasp"); - robot_state::RobotStatePtr pre_grasp_state = - std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState()); - valid_grasp_candidate->getPreGraspState(pre_grasp_state); - visual_tools_->publishRobotState(pre_grasp_state, rviz_visual_tools::ORANGE); - robot_state::RobotStatePtr grasp_state = - std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState()); - if (valid_grasp_candidate->getGraspStateClosed(grasp_state)) - { - visual_tools_->prompt("grasp"); - visual_tools_->publishRobotState(grasp_state, rviz_visual_tools::YELLOW); - } - if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 1 && - valid_grasp_candidate->segmented_cartesian_traj_[1].size()) - { - visual_tools_->prompt("lift"); - visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[1].back(), - rviz_visual_tools::BLUE); - } - if (valid_grasp_candidate->segmented_cartesian_traj_.size() > 2 && - valid_grasp_candidate->segmented_cartesian_traj_[2].size()) - { - visual_tools_->prompt("retreat"); - visual_tools_->publishRobotState(valid_grasp_candidate->segmented_cartesian_traj_[2].back(), - rviz_visual_tools::PURPLE); - } - - visual_tools_->prompt("show free space approach"); - visual_tools_->hideRobot(); - visual_tools_->trigger(); - - bool wait_for_animation = true; - visual_tools_->publishTrajectoryPath(pre_approach_plan.trajectory, pre_grasp_state, wait_for_animation); - ros::Duration(0.25).sleep(); - if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::APPROACH) - visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH], - valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); - ros::Duration(0.25).sleep(); - - if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::LIFT) - visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::LIFT], - valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); - ros::Duration(0.25).sleep(); - - if (valid_grasp_candidate->segmented_cartesian_traj_.size() > moveit_grasps::RETREAT) - visual_tools_->publishTrajectoryPath(valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::RETREAT], - valid_grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); - ros::Duration(0.25).sleep(); - } - */ - - bool planFullGrasp(std::vector<moveit_grasps::GraspCandidatePtr>& grasp_candidates, - moveit_grasps::GraspCandidatePtr& valid_grasp_candidate, - moveit_msgs::MotionPlanResponse& pre_approach_plan, const std::string& object_name) - { - moveit::core::RobotStatePtr current_state; - { - boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls( - new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_)); - current_state = std::make_shared<robot_state::RobotState>((*ls)->getCurrentState()); - } - - bool success = false; - for (; !grasp_candidates.empty(); grasp_candidates.erase(grasp_candidates.begin())) - { - valid_grasp_candidate = grasp_candidates.front(); - valid_grasp_candidate->getPreGraspState(current_state); - if (!grasp_planner_->planApproachLiftRetreat(valid_grasp_candidate, current_state, planning_scene_monitor_, false, - object_name)) - { - ROS_INFO_NAMED(LOGNAME, "failed to plan approach lift retreat"); - continue; - } - - robot_state::RobotStatePtr pre_grasp_state = - valid_grasp_candidate->segmented_cartesian_traj_[moveit_grasps::APPROACH].front(); - if (!planPreApproach(*pre_grasp_state, pre_approach_plan)) - { - ROS_WARN_NAMED(LOGNAME, "failed to plan to pregrasp_state"); - continue; - } - - success = true; - break; - } - return success; - } - - bool planPreApproach(const robot_state::RobotState& goal_state, moveit_msgs::MotionPlanResponse& pre_approach_plan) - { - planning_interface::MotionPlanRequest req; - planning_interface::MotionPlanResponse res; - - double tolerance_above = 0.01; - double tolerance_below = 0.01; - // req.planner_id = "RRTConnectkConfigDefault"; - req.group_name = arm_jmg_->getName(); - req.num_planning_attempts = 5; - req.allowed_planning_time = 1.5; - moveit_msgs::Constraints goal = - kinematic_constraints::constructGoalConstraints(goal_state, arm_jmg_, tolerance_below, tolerance_above); - - req.goal_constraints.push_back(goal); - boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls( - new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_)); - - // --------------------------------- - // Change the robot current state - // NOTE: We have to do this since Panda start configuration is in self collision. - robot_state::RobotState rs = (*ls)->getCurrentState(); - robot_state::robotStateToRobotStateMsg(rs, req.start_state); - // --------------------------- - - planning_pipeline_->generatePlan(*ls, req, res); - if (res.error_code_.val != res.error_code_.SUCCESS) - { - ROS_INFO_NAMED(LOGNAME, "Failed to plan approach successfully"); - return false; - } - - res.getMessage(pre_approach_plan); - return true; - } - - bool getIKSolution(const moveit::core::JointModelGroup* arm_jmg, const Eigen::Isometry3d& target_pose, - robot_state::RobotState& solution, const std::string& link_name, geometry_msgs::Point& original_target) - { - boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRW> ls( - new planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_)); - - moveit::core::GroupStateValidityCallbackFn constraint_fn = boost::bind( - &isStateValid, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get(), visual_tools_, _1, _2, _3); - - // seed IK call with current state - solution = (*ls)->getCurrentState(); - - - // Solve IK problem for arm - // disable explicit restarts to guarantee close solution if one exists - const double timeout = 0.1; - - - - - - if (solution.setFromIK(arm_jmg, target_pose, link_name, timeout, constraint_fn)) { - geometry_msgs::Pose target = tf2::toMsg(target_pose); - node["data"].push_back(std::string( - std::to_string(original_target.x) + " " - + std::to_string(original_target.y) + " " - + std::to_string(original_target.z) + " " - + std::to_string(target.orientation.x) + " " - + std::to_string(target.orientation.y) + " " - + std::to_string(target.orientation.z) + " " - + std::to_string(target.orientation.w) - )); - return true; - } - return false; - } - - bool generateRandomCuboid(std::string& object_name, geometry_msgs::Pose& object_pose, double& x_depth, - double& y_width, double& z_height) - { - - // Generate random cuboid - double xmin = 0.5; - double xmax = 0.7; - double ymin = -0.25; - double ymax = 0.25; - double zmin = 0.2; - double zmax = 0.7; - rviz_visual_tools::RandomPoseBounds pose_bounds(xmin, xmax, ymin, ymax, zmin, zmax); - - double cuboid_size_min = 0.01; - double cuboid_size_max = 0.03; - rviz_visual_tools::RandomCuboidBounds cuboid_bounds(cuboid_size_min, cuboid_size_max); - - object_name = "pick_target"; - visual_tools_->generateRandomCuboid(object_pose, x_depth, y_width, z_height, pose_bounds, cuboid_bounds); - visual_tools_->publishCollisionCuboid(object_pose, x_depth, y_width, z_height, object_name, rviz_visual_tools::RED); - visual_tools_->publishAxis(object_pose, rviz_visual_tools::MEDIUM); - visual_tools_->trigger(); - - bool success = true; - double timeout = 5; // seconds - ros::Rate rate(100); - while (success && !planning_scene_monitor_->getPlanningScene()->knowsFrameTransform(object_name)) - { - rate.sleep(); - success = rate.cycleTime().toSec() < timeout; - } - return success; - } - - bool reachGrasp (float& x, float& y, float& z , std::string& object_name, double& x_depth, double& y_depth, double& z_depth) { - // Generate random cuboid - geometry_msgs::Pose object_pose; - geometry_msgs::Point point; - double xmin = 0.5; - double xmax = 0.7; - double ymin = -0.25; - double ymax = 0.25; - double zmin = 0.2; - double zmax = 0.7; - rviz_visual_tools::RandomPoseBounds pose_bounds(xmin, xmax, ymin, ymax, zmin, zmax); - - double cuboid_size_min = 0.01; - double cuboid_size_max = 0.03; - rviz_visual_tools::RandomCuboidBounds cuboid_bounds(cuboid_size_min, cuboid_size_max); - - visual_tools_->generateRandomCuboid(object_pose, x_depth, y_depth, z_depth, pose_bounds, cuboid_bounds); - - point.x = object_pose.position.x = x; - point.y = object_pose.position.y = y; - point.z = object_pose.position.z = z; - - visual_tools_->publishCollisionCuboid(object_pose, x_depth, y_depth, z_depth, object_name, rviz_visual_tools::RED); - visual_tools_->publishAxis(object_pose, rviz_visual_tools::MEDIUM); - visual_tools_->trigger(); - - bool success = true; - double timeout = 5; - ros::Rate rate(100); - - while (success && !planning_scene_monitor_->getPlanningScene()->knowsFrameTransform(object_name)){ - rate.sleep(); - success = rate.cycleTime().toSec() < timeout; - } - - if (!success){ - ROS_ERROR_NAMED(LOGNAME, "Something went wring in Cube construction ._."); - return false; - } - - // ----------------------------------- - // Generate grasp candidates - std::vector<moveit_grasps::GraspCandidatePtr> grasp_candidates; - - // Configure the desired types of grasps - moveit_grasps::TwoFingerGraspCandidateConfig grasp_generator_config = - moveit_grasps::TwoFingerGraspCandidateConfig(); - grasp_generator_config.disableAll(); - grasp_generator_config.enable_face_grasps_ = true; - grasp_generator_config.generate_y_axis_grasps_ = true; - grasp_generator_config.generate_x_axis_grasps_ = true; - grasp_generator_config.generate_z_axis_grasps_ = true; - - grasp_generator_->setGraspCandidateConfig(grasp_generator_config); - if (!grasp_generator_->generateGrasps(visual_tools_->convertPose(object_pose), x_depth, y_depth, - z_depth, grasp_data_, grasp_candidates)){ - ROS_ERROR_NAMED(LOGNAME, "Something went wrong in grasp generation"); - return false; - } - - // -------------------------------------------- - // Generating a seed state for filtering grasps - robot_state::RobotStatePtr seed_state = - std::make_shared<robot_state::RobotState>(*visual_tools_->getSharedRobotState()); - Eigen::Isometry3d eef_mount_grasp_pose = - visual_tools_->convertPose(object_pose) * grasp_data_->tcp_to_eef_mount_.inverse(); - if (!getIKSolution(arm_jmg_, eef_mount_grasp_pose, *seed_state, grasp_data_->parent_link_->getName(),point)) - { - ROS_WARN_STREAM_NAMED(LOGNAME, "The ideal seed state is not reachable. Using start state as seed."); - } - - // -------------------------------------------- - // Filtering grasps - // Note: This step also solves for the grasp and pre-grasp states and stores them in grasp candidates) - bool filter_pregrasps = true; - if (!grasp_filter_->filterGrasps(grasp_candidates, planning_scene_monitor_, arm_jmg_, seed_state, filter_pregrasps, - object_name)){ - ROS_ERROR_STREAM_NAMED(LOGNAME, "Filter grasps failed"); - return false; - } - if (!grasp_filter_->removeInvalidAndFilter(grasp_candidates)){ - ROS_WARN_NAMED(LOGNAME, "Grasp filtering removed all grasps"); - return false; - } - ROS_INFO_STREAM_NAMED(LOGNAME, "" << grasp_candidates.size() << " remain after filtering"); - - // Plan free-space approach, cartesian approach, lift and retreat trajectories - /* - moveit_grasps::GraspCandidatePtr selected_grasp_candidate; - moveit_msgs::MotionPlanResponse pre_approach_plan; - if (!planFullGrasp(grasp_candidates, selected_grasp_candidate, pre_approach_plan, object_name)){ - ROS_ERROR_STREAM_NAMED(LOGNAME, "Failed to plan grasp motions"); - return false; - } - */ - - - return true; - } - -private: - // A shared node handle - ros::NodeHandle nh_; - - // Tool for visualizing things in Rviz - moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; - - // MoveIt! Grasps - moveit_grasps::TwoFingerGraspGeneratorPtr grasp_generator_; - - // Robot-specific data for generating grasps - moveit_grasps::TwoFingerGraspDataPtr grasp_data_; - - // For planning approach and retreats - moveit_grasps::GraspPlannerPtr grasp_planner_; - - // For selecting good grasps - moveit_grasps::TwoFingerGraspFilterPtr grasp_filter_; - - // Shared planning scene (load once for everything) - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - // Arm - const robot_model::JointModelGroup* arm_jmg_; - - // Robot - robot_model::RobotModelPtr robot_model_; - - // All the motion planning components - planning_pipeline::PlanningPipelinePtr planning_pipeline_; - - // Choose which arm to use - std::string ee_group_name_; - std::string planning_group_name_; - -}; // end of class - -} // namespace moveit_grasps - -int main(int argc, char* argv[]) -{ - int num_tests = 1; - - ros::init(argc, argv, "grasp_filter_demo"); - - // Allow the action server to recieve and send ros messages - ros::AsyncSpinner spinner(2); - spinner.start(); - - // Seed random - srand(ros::Time::now().toSec()); - - // Benchmark time - ros::Time start_time; - start_time = ros::Time::now(); - - // Head of yaml - - - // Guess I need this - double x_depth = 0.2; - double y_depth = 0.4; - double z_depth = 0.4; - float dimension_x = 0.9; - float dimension_y = 0.9; - float dimension_z = 1.2; - - float resolution = 0.2; - - moveit_grasps::GraspPipeline tester; - std::string object_name = "pick_target"; - - for (float x = 0 - dimension_x; x <= -0.22f + dimension_x; x+= resolution){ - for(float y = 0 - dimension_y; y <= 0 + dimension_y; y+= resolution){ - for(float z = 0.885f; z <= 0.885f + dimension_z; z+= resolution){ - tester.reachGrasp(x,y,z, object_name, x_depth, y_depth, z_depth); - } - } - } - - - - - // Benchmark time - double duration = (ros::Time::now() - start_time).toSec(); - - node["description"]["system"] = "Franka Emika 'Panda'"; - node["description"]["resolution"] = resolution; - node["description"]["dimension"] = std::to_string(dimension_x) + " " + std::to_string(dimension_y) + " " + std::to_string(dimension_z) + " (m)"; - node["description"]["timestamp"] = std::to_string(duration) + " sec"; - - std::ofstream file(ros::package::getPath("mtc") + "/maps/dummy.yaml"); - file << node; - - - ROS_INFO_STREAM_NAMED("grasp_filter_demo", "Total time: " << duration << "\t" << num_tests); - std::cout << "Total time: " << duration << "\t" << num_tests << std::endl; - - ros::Duration(1.0).sleep(); // let rviz markers finish publishing - - return 0; -} diff --git a/src/mtc/src/grasp_planner.cpp b/src/mtc/src/grasp_planner.cpp deleted file mode 100644 index 4cb443cf218ca23386d93795f7726cf1c1b3c96f..0000000000000000000000000000000000000000 --- a/src/mtc/src/grasp_planner.cpp +++ /dev/null @@ -1,435 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman <dave@picknik.ai> - Desc: Find the approach, lift, and retreat path for a candidate grasp (if a valid one exists) -*/ - -// moveit_grasps -#include "../include/grasp_planner.h" -#include "../include/state_validity_callback.h" -#include <moveit/robot_state/cartesian_interpolator.h> - -// Parameter loading -#include <rosparam_shortcuts/rosparam_shortcuts.h> - -namespace moveit_grasps -{ -constexpr char ENABLED_PARENT_NAME[] = "grasp_planner"; // for namespacing logging messages -constexpr char ENABLED_SETTINGS_NAMESPACE[] = "moveit_grasps/planner"; - -GraspPlanner::GraspPlanner(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools) - : nh_("~"), visual_tools_(visual_tools) -{ - loadEnabledSettings(); -} - -bool GraspPlanner::planAllApproachLiftRetreat( - std::vector<GraspCandidatePtr>& grasp_candidates, const robot_state::RobotStatePtr& robot_state, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, const std::string& grasp_object_id) -{ - boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls; - ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor)); - return planAllApproachLiftRetreat(grasp_candidates, robot_state, - static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls), grasp_object_id); -} - -bool GraspPlanner::planAllApproachLiftRetreat(std::vector<GraspCandidatePtr>& grasp_candidates, - const robot_state::RobotStatePtr& robot_state, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const std::string& grasp_object_id) -{ - ROS_INFO_STREAM_NAMED("grasp_planner", "Planning all remaining grasps with approach lift retreat cartesian path"); - - // For each remaining grasp, calculate entire approach, lift, and retreat path. - // Remove those that have no valid path - bool verbose_cartesian_filtering = isEnabled("verbose_cartesian_filtering"); - std::size_t grasp_candidates_before_cartesian_path = grasp_candidates.size(); - - std::size_t count = 0; - for (std::vector<GraspCandidatePtr>::iterator grasp_it = grasp_candidates.begin(); grasp_it != grasp_candidates.end();) - { - if (!ros::ok()) - return false; - - if (isEnabled("verbose_cartesian_filtering")) - { - ROS_INFO_STREAM_NAMED("grasp_planner", ""); - ROS_INFO_STREAM_NAMED("grasp_planner", "Attempting to plan cartesian grasp path #" - << count++ << ". " << grasp_candidates.size() << " remaining."); - } - - if (!planApproachLiftRetreat(*grasp_it, robot_state, planning_scene, verbose_cartesian_filtering, grasp_object_id)) - { - ROS_INFO_STREAM_NAMED("grasp_planner", "Grasp candidate was unable to find valid cartesian waypoint path"); - - grasp_it = grasp_candidates.erase(grasp_it); // not valid - } - else - { - ++grasp_it; // move to next grasp - } - - if (isEnabled("show_cartesian_waypoints")) - { - visual_tools_->deleteAllMarkers(); - visual_tools_->trigger(); - } - } - - // Results - if (isEnabled("statistics_verbose")) - { - std::cout << std::endl; - std::cout << "-------------------------------------------------------" << std::endl; - std::cout << "Total grasp candidates: " << grasp_candidates_before_cartesian_path << std::endl; - std::cout << "Failed due to invalid cartesian path: " - << grasp_candidates_before_cartesian_path - grasp_candidates.size() << std::endl; - std::cout << "Remaining grasp candidates: " << grasp_candidates.size() << std::endl; - std::cout << "-------------------------------------------------------" << std::endl; - std::cout << std::endl; - } - - // If no grasp candidates had valid paths, then we return false - if (grasp_candidates.size() == 0) - { - ROS_DEBUG_STREAM_NAMED("grasp_planner", "No valid grasp plan possible"); - return false; - } - return true; -} - -bool GraspPlanner::planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, - const robot_state::RobotStatePtr& robot_state, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - bool verbose_cartesian_filtering, const std::string& grasp_object_id) -{ - boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls; - ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor)); - return planApproachLiftRetreat(grasp_candidate, robot_state, - static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls), - verbose_cartesian_filtering, grasp_object_id); -} - -bool GraspPlanner::planApproachLiftRetreat(GraspCandidatePtr& grasp_candidate, - const robot_state::RobotStatePtr& robot_state, - const planning_scene::PlanningSceneConstPtr& planning_scene, - bool verbose_cartesian_filtering, const std::string& grasp_object_id) -{ - EigenSTL::vector_Isometry3d waypoints; - GraspGenerator::getGraspWaypoints(grasp_candidate, waypoints); - - // Visualize waypoints - bool show_cartesian_waypoints = isEnabled("show_cartesian_waypoints"); - if (show_cartesian_waypoints) - { - visual_tools_->publishAxisLabeled(waypoints[0], "pregrasp"); - visual_tools_->publishAxisLabeled(waypoints[1], "grasp"); - visual_tools_->publishAxisLabeled(waypoints[2], "lifted"); - visual_tools_->publishAxisLabeled(waypoints[3], "retreat"); - visual_tools_->trigger(); - - if (grasp_candidate->getPreGraspState(visual_tools_->getSharedRobotState())) - { - waitForNextStep("see pre grasp state"); - visual_tools_->publishRobotState(visual_tools_->getSharedRobotState(), rviz_visual_tools::RED); - visual_tools_->trigger(); - } - if (grasp_candidate->getGraspStateClosed(visual_tools_->getSharedRobotState())) - { - waitForNextStep("see grasp state"); - visual_tools_->publishRobotState(visual_tools_->getSharedRobotState(), rviz_visual_tools::BLUE); - visual_tools_->trigger(); - } - waitForNextStep("continue cartesian planning"); - } - - // Remove the pre-grasp point since we plan from that start state - waypoints.erase(waypoints.begin()); - - // Starting state - moveit::core::RobotStatePtr start_state(new moveit::core::RobotState(*robot_state)); - if (!grasp_candidate->getPreGraspState(start_state)) - { - ROS_WARN_STREAM_NAMED("grasp_planner.waypoints", "Unable to set pregrasp"); - return false; - } - - if (!computeCartesianWaypointPath(grasp_candidate, planning_scene, start_state, waypoints, grasp_object_id)) - { - ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Unable to plan approach lift retreat path"); - - return false; - } - - // Feedback - ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Found valid and complete waypoint manipulation path for grasp " - "candidate"); - - // Show visuals - if (show_cartesian_waypoints) - { - ROS_INFO_STREAM_NAMED("grasp_planner.waypoints", "Visualize end effector position of cartesian path for " - << grasp_candidate->segmented_cartesian_traj_.size() - << " segments"); - visual_tools_->publishTrajectoryPoints(grasp_candidate->segmented_cartesian_traj_[APPROACH], - grasp_candidate->grasp_data_->parent_link_, rviz_visual_tools::YELLOW); - visual_tools_->publishTrajectoryPoints(grasp_candidate->segmented_cartesian_traj_[LIFT], - grasp_candidate->grasp_data_->parent_link_, rviz_visual_tools::ORANGE); - visual_tools_->publishTrajectoryPoints(grasp_candidate->segmented_cartesian_traj_[RETREAT], - grasp_candidate->grasp_data_->parent_link_, rviz_visual_tools::RED); - visual_tools_->trigger(); - - bool wait_for_animation = true; - visual_tools_->publishTrajectoryPath(grasp_candidate->segmented_cartesian_traj_[APPROACH], - grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); - visual_tools_->publishTrajectoryPath(grasp_candidate->segmented_cartesian_traj_[LIFT], - grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); - visual_tools_->publishTrajectoryPath(grasp_candidate->segmented_cartesian_traj_[RETREAT], - grasp_candidate->grasp_data_->arm_jmg_, wait_for_animation); - } - - if (verbose_cartesian_filtering) - waitForNextStep("try next candidate grasp"); - - return true; -} - -bool GraspPlanner::computeCartesianWaypointPath( - GraspCandidatePtr& grasp_candidate, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const moveit::core::RobotStatePtr& start_state, const EigenSTL::vector_Isometry3d& waypoints, - const std::string& grasp_object_id) -{ - boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls; - ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor)); - - return computeCartesianWaypointPath(grasp_candidate, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls), - start_state, waypoints, grasp_object_id); -} - -bool GraspPlanner::computeCartesianWaypointPath(GraspCandidatePtr& grasp_candidate, - const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit::core::RobotStatePtr& start_state, - const EigenSTL::vector_Isometry3d& waypoints, - const std::string& grasp_object_id) -{ - // End effector parent link (arm tip for ik solving) - const moveit::core::LinkModel* ik_tip_link = grasp_candidate->grasp_data_->parent_link_; - - // Resolution of trajectory - // The maximum distance in Cartesian space between consecutive points on the resulting path - const double max_step = 0.01; - - // Jump threshold for preventing consequtive joint values from 'jumping' by a large amount in joint space - const double jump_threshold = 4; // config_->jump_threshold_; // aka jump factor - - // Collision setting - const bool collision_checking_verbose = isEnabled("collision_checking_verbose"); - const bool only_check_self_collision = false; - - // Reference frame setting - const bool global_reference_frame = true; - - // Check for kinematic solver - if (!grasp_candidate->grasp_data_->arm_jmg_->canSetStateFromIK(ik_tip_link->getName())) - { - ROS_ERROR_STREAM_NAMED("grasp_planner.waypoints", "No IK Solver loaded - make sure moveit_config/kinamatics.yaml " - "is loaded in this namespace"); - return false; - } - - std::size_t attempts = 0; - static const std::size_t MAX_IK_ATTEMPTS = 5; - bool valid_path_found = false; - while (attempts < MAX_IK_ATTEMPTS) - { - if (attempts > 0) - { - ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Attempting IK solution, attempt # " << attempts + 1); - } - attempts++; - - moveit::core::RobotStatePtr start_state_copy(new moveit::core::RobotState(*start_state)); - if (!grasp_candidate->getPreGraspState(start_state_copy)) - { - ROS_ERROR_STREAM_NAMED("grasp_planner.waypoints", "Unable to set pregrasp"); - return false; - } - - // Create a clone where we modify the ACM to disable collision checking between the end effector and object - planning_scene::PlanningScenePtr scene = planning_scene::PlanningScene::clone(planning_scene); - // If the grasp_object_id is set then we disable collision checking in the planning scene clone - if (!grasp_object_id.empty() && scene->knowsFrameTransform(grasp_object_id)) - { - std::vector<std::string> ee_link_names = grasp_candidate->grasp_data_->ee_jmg_->getLinkModelNames(); - for (const auto& ee_link : ee_link_names) - scene->getAllowedCollisionMatrixNonConst().setEntry(grasp_object_id, ee_link, true); - } - // Collision check - moveit::core::GroupStateValidityCallbackFn constraint_fn = - boost::bind(&isGraspStateValid, scene.get(), collision_checking_verbose, only_check_self_collision, - visual_tools_, _1, _2, _3); - - // Compute Cartesian Path - grasp_candidate->segmented_cartesian_traj_.clear(); - grasp_candidate->segmented_cartesian_traj_.resize(3); - double valid_approach_percentage = robot_state::CartesianInterpolator::computeCartesianPath( - start_state_copy.get(), grasp_candidate->grasp_data_->arm_jmg_, - grasp_candidate->segmented_cartesian_traj_[APPROACH], ik_tip_link, waypoints[APPROACH], global_reference_frame, - robot_state::MaxEEFStep(max_step), robot_state::JumpThreshold(jump_threshold), constraint_fn, - kinematics::KinematicsQueryOptions()); - - if (!grasp_candidate->getGraspStateClosedEEOnly(start_state_copy)) - { - ROS_ERROR_STREAM_NAMED("grasp_planner", "Unable to set pregrasp"); - return false; - } - - // Attach CO to end effector - if (!grasp_object_id.empty() && scene->knowsFrameTransform(grasp_object_id)) - { - // Create an AttachedCollisionObject - moveit_msgs::AttachedCollisionObject aco; - - // Create a reference to the collision object for convenience - moveit_msgs::CollisionObject& suction_voxel_co = aco.object; - - suction_voxel_co.id = grasp_object_id; - suction_voxel_co.header.frame_id = ik_tip_link->getName(); - - // Set the aco attached link name - aco.link_name = ik_tip_link->getName(); - - // move the robot in the ps to the end of the approach path. - scene->setCurrentState(*start_state_copy); - - // Mark object to be added - suction_voxel_co.operation = moveit_msgs::CollisionObject::ADD; - if (!scene->processAttachedCollisionObjectMsg(aco)) - { - ROS_WARN_STREAM_NAMED("grasp_planner", "Failed to attach: " << aco.object.id); - } - else - { - auto start_state_with_body = std::make_shared<robot_state::RobotState>(scene->getCurrentState()); - std::vector<const robot_state::AttachedBody*> attached_bodies; - start_state_with_body->getAttachedBodies(attached_bodies); - for (const auto& ab : attached_bodies) - start_state_copy->attachBody(ab->getName(), ab->getPose(), ab->getShapes(), ab->getShapePoses(), - ab->getTouchLinks(), ab->getAttachedLinkName(), ab->getDetachPosture(), - ab->getSubframes()); - constraint_fn = boost::bind(&isGraspStateValid, scene.get(), collision_checking_verbose, - only_check_self_collision, visual_tools_, _1, _2, _3); - } - } - - double valid_lift_retreat_percentage = robot_state::CartesianInterpolator::computeCartesianPath( - start_state_copy.get(), grasp_candidate->grasp_data_->arm_jmg_, - grasp_candidate->segmented_cartesian_traj_[LIFT], ik_tip_link, waypoints[LIFT], global_reference_frame, - robot_state::MaxEEFStep(max_step), robot_state::JumpThreshold(jump_threshold), constraint_fn, - kinematics::KinematicsQueryOptions()); - - valid_lift_retreat_percentage *= robot_state::CartesianInterpolator::computeCartesianPath( - start_state_copy.get(), grasp_candidate->grasp_data_->arm_jmg_, - grasp_candidate->segmented_cartesian_traj_[RETREAT], ik_tip_link, waypoints[RETREAT], global_reference_frame, - robot_state::MaxEEFStep(max_step), robot_state::JumpThreshold(jump_threshold), constraint_fn, - kinematics::KinematicsQueryOptions()); - - ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "valid_approach_percentage: " << valid_approach_percentage - << " \tvalid_lift_retreat_" - "percentage: " - << valid_lift_retreat_percentage); - - // The retreat has to work for the most part but doesn't need to be perfect - double min_allowed_valid_lift_retreat_percentage = 0.90; - if (valid_approach_percentage == 1 && valid_lift_retreat_percentage >= min_allowed_valid_lift_retreat_percentage) - { - ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", "Found valid cartesian path"); - valid_path_found = true; - break; - } - } // end while AND scoped pointer of locked planning scenep - - if (!valid_path_found) - { - ROS_DEBUG_STREAM_NAMED("grasp_planner.waypoints", - "UNABLE to find valid waypoint cartesian path after " << MAX_IK_ATTEMPTS << " attempts"); - return false; - } - - return true; -} - -void GraspPlanner::waitForNextStep(const std::string& message) -{ - if (wait_for_next_step_callback_) - wait_for_next_step_callback_(message); -} - -void GraspPlanner::setWaitForNextStepCallback(WaitForNextStepCallback callback) -{ - wait_for_next_step_callback_ = callback; -} - -bool GraspPlanner::loadEnabledSettings() -{ - // Check if the map has been loaded yet - if (!enabled_settings_loaded_) - { - enabled_settings_loaded_ = true; - return rosparam_shortcuts::get(ENABLED_PARENT_NAME, nh_, ENABLED_SETTINGS_NAMESPACE, enabled_setting_); - } - return true; -} - -bool GraspPlanner::isEnabled(const std::string& setting_name) -{ - // Check if the map has been loaded yet. it is preferred if this is called manually - if (!enabled_settings_loaded_) - ROS_ERROR_STREAM_NAMED("rosparam_shortcuts", "Enabled settings are not yet loaded e.g. call loadEnabledSettings()"); - - std::map<std::string, bool>::iterator it = enabled_setting_.find(setting_name); - if (it != enabled_setting_.end()) - { - // Element found; - return it->second; - } - ROS_ERROR_STREAM_NAMED("rosparam_shortcuts", "isEnabled() key '" << nh_.getNamespace() << "/" - << ENABLED_SETTINGS_NAMESPACE << "/" << setting_name - << "' does not exist on the parameter server"); - - return false; -} - -} // namespace moveit_grasps diff --git a/src/mtc/src/grasp_scorer.cpp b/src/mtc/src/grasp_scorer.cpp deleted file mode 100644 index 934e4832e61a7d44a12006b281cc4aaece2d255c..0000000000000000000000000000000000000000 --- a/src/mtc/src/grasp_scorer.cpp +++ /dev/null @@ -1,165 +0,0 @@ -/********************************************************************* - * Software License Agreement ("Modified BSD License") - * - * Copyright (c) 2014, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/** - * Authors : Andy McEvoy ( mcevoy.andy@gmail.com ) - * Desc : Functions for scoring grasps. See *.h file for documentation - */ - -#include "../include/grasp_scorer.h" - -namespace moveit_grasps -{ -double GraspScoreWeights::computeScore(const Eigen::Vector3d& orientation_scores, - const Eigen::Vector3d& translation_scores, bool verbose) const -{ - double total_score = - orientation_scores[0] * orientation_x_score_weight_ + orientation_scores[1] * orientation_y_score_weight_ + - orientation_scores[2] * orientation_z_score_weight_ + translation_scores[0] * translation_x_score_weight_ + - translation_scores[1] * translation_y_score_weight_ + translation_scores[2] * translation_z_score_weight_; - - total_score /= getWeightTotal(); - - if (verbose) - { - static const std::string logger_name = "grasp_scorer.compute_score"; - ROS_DEBUG_STREAM_NAMED(logger_name, "Grasp score: "); - // clang-format off - ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.x = " << orientation_scores[0] << "\tweight = "<< orientation_x_score_weight_); - ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.y = " << orientation_scores[1] << "\tweight = "<< orientation_y_score_weight_); - ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.z = " << orientation_scores[2] << "\tweight = "<< orientation_z_score_weight_); - ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.x = " << translation_scores[0] << "\tweight = "<< translation_x_score_weight_); - ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.y = " << translation_scores[1] << "\tweight = "<< translation_y_score_weight_); - ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.z = " << translation_scores[2] << "\tweight = "<< translation_z_score_weight_); - // Total - ROS_DEBUG_STREAM_NAMED(logger_name, "\ttotal_score = " << total_score); - // clang-format on - } - return total_score; -} - -double GraspScoreWeights::getWeightTotal() const -{ - return orientation_x_score_weight_ + orientation_y_score_weight_ + orientation_z_score_weight_ + - translation_x_score_weight_ + translation_y_score_weight_ + translation_z_score_weight_; -} - -Eigen::Vector3d GraspScorer::scoreRotationsFromDesired(const Eigen::Isometry3d& grasp_pose_tcp, - const Eigen::Isometry3d& ideal_pose) -{ - Eigen::Vector3d grasp_pose_axis; - Eigen::Vector3d ideal_pose_axis; - Eigen::Vector3d scores; - double cos_angle; - double angle; - - // get angle between x-axes - grasp_pose_axis = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitX(); - ideal_pose_axis = ideal_pose.rotation() * Eigen::Vector3d::UnitX(); - cos_angle = grasp_pose_axis.dot(ideal_pose_axis); - angle = acos(std::max(-1.0, std::min(1.0, cos_angle))); - ROS_DEBUG_STREAM_NAMED("grasp_scorer.angle", "x angle = " << angle * 180.0 / M_PI); - scores[0] = (M_PI - angle) / M_PI; - - // get angle between y-axes - grasp_pose_axis = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitY(); - ideal_pose_axis = ideal_pose.rotation() * Eigen::Vector3d::UnitY(); - cos_angle = grasp_pose_axis.dot(ideal_pose_axis); - angle = acos(std::max(-1.0, std::min(1.0, cos_angle))); - ROS_DEBUG_STREAM_NAMED("grasp_scorer.angle", "y angle = " << angle * 180.0 / M_PI); - scores[1] = (M_PI - angle) / M_PI; - - // get angle between z-axes - grasp_pose_axis = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ(); - ideal_pose_axis = ideal_pose.rotation() * Eigen::Vector3d::UnitZ(); - cos_angle = grasp_pose_axis.dot(ideal_pose_axis); - angle = acos(std::max(-1.0, std::min(1.0, cos_angle))); - ROS_DEBUG_STREAM_NAMED("grasp_scorer.angle", "z angle = " << angle * 180.0 / M_PI); - scores[2] = (M_PI - angle) / M_PI; - - return scores; -} - -Eigen::Vector3d GraspScorer::scoreGraspTranslation(const Eigen::Isometry3d& grasp_pose_tcp, - const Eigen::Isometry3d& ideal_pose) -{ - // We assume that the ideal is in the middle - Eigen::Vector3d scores = -Eigen::Vector3d(grasp_pose_tcp.translation() - ideal_pose.translation()).array().abs(); - - ROS_DEBUG_STREAM_NAMED("grasp_scorer.scoreGraspTranslation", - "value, ideal, score:\n" - << "x: " << grasp_pose_tcp.translation()[0] << "\t" << ideal_pose.translation()[0] << "\t" - << scores[0] << "\n" - << "y: " << grasp_pose_tcp.translation()[1] << "\t" << ideal_pose.translation()[1] << "\t" - << scores[1] << "\n" - << "x: " << grasp_pose_tcp.translation()[2] << "\t" << ideal_pose.translation()[2] << "\t" - << scores[2] << "\n"); - - return scores; -} - -Eigen::Vector3d GraspScorer::scoreGraspTranslation(const Eigen::Isometry3d& grasp_pose_tcp, - const Eigen::Vector3d& min_translations, - const Eigen::Vector3d& max_translations) -{ - Eigen::Vector3d scores; - - for (std::size_t i = 0; i < 3; ++i) - { - // We assume that the ideal is in the middle - double ideal = (max_translations[i] + min_translations[i]) / 2; - double translation = grasp_pose_tcp.translation()[i] - ideal; - double range = max_translations[i] - min_translations[i]; - double score; - if (range == 0) - score = 0; - else - score = translation / range; - - scores[i] = pow(score, 2); - } - - ROS_DEBUG_STREAM_NAMED("grasp_scorer.translation", - "\nvalue, min, max, score:\n" - << grasp_pose_tcp.translation()[0] << ", " << min_translations[0] << ", " - << max_translations[0] << ", " << scores[0] << "\n" - << grasp_pose_tcp.translation()[1] << ", " << min_translations[1] << ", " - << max_translations[1] << ", " << scores[1] << "\n" - << grasp_pose_tcp.translation()[2] << ", " << min_translations[2] << ", " - << max_translations[2] << ", " << scores[2] << "\n"); - - return scores; -} - -} // namespace moveit_grasps diff --git a/src/mtc/src/impl/mediator.cpp b/src/mtc/src/impl/mediator.cpp index 807148f8e1bd13ceb287c0b52c1fc51df956876d..0730159559b2642a1a3e4d92dcced78ce8b7669d 100644 --- a/src/mtc/src/impl/mediator.cpp +++ b/src/mtc/src/impl/mediator.cpp @@ -294,16 +294,19 @@ void Mediator::write_file(Robot* A, Robot* B){ tf2::Matrix3x3 m(A->tf().getRotation()); m.getRPY(r,p,y); + + float size_x = A->size().getX(); + float size_y = A->size().getY(); + float size_z = A->size().getZ(); + float pos_x = A->tf().getOrigin().getX(); float pos_y = A->tf().getOrigin().getY(); - float pos_z = A->tf().getOrigin().getZ() *2; + float pos_z = A->tf().getOrigin().getZ() *2 - size_z/2; float rot_x = A->tf().getRotation().getX(); float rot_y = A->tf().getRotation().getY(); float rot_z = A->tf().getRotation().getZ(); float rot_w = A->tf().getRotation().getW(); - float size_x = A->size().getX(); - float size_y = A->size().getY(); - float size_z = A->size().getZ(); + @@ -323,16 +326,17 @@ void Mediator::write_file(Robot* A, Robot* B){ m.setRotation(B->tf().getRotation()); m.getRPY(r,p,y); + size_x = B->size().getX(); + size_y = B->size().getY(); + size_z = B->size().getZ(); + pos_x = B->tf().getOrigin().getX(); pos_y = B->tf().getOrigin().getY(); - pos_z = B->tf().getOrigin().getZ() *2; + pos_z = B->tf().getOrigin().getZ() *2 - size_z/2; rot_x = B->tf().getRotation().getX(); rot_y = B->tf().getRotation().getY(); rot_z = B->tf().getRotation().getZ(); rot_w = B->tf().getRotation().getW(); - size_x = B->size().getX(); - size_y = B->size().getY(); - size_z = B->size().getZ(); ss << "{ 'id' : 'table" << B->name().back() << "_wheel_1', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }}, \n"; ss << "{ 'id' : 'table" << B->name().back() << "_wheel_2', 'pos': { 'x': 0,'y': 0,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 }},\n"; @@ -360,7 +364,7 @@ void Mediator::write_file(Robot* A, Robot* B){ float width = w->size().getY(); float height = w->size().getZ(); - ss << "{ 'id': 'table" << A->name().back() << "_" << w->name() << "' , 'pos': { 'x': "<< std::to_string(x) << " , 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z + height/2) << " } , 'size': { 'length': "<< std::to_string(length) << " , 'width': "<< std::to_string(width) << " , 'height': "<< std::to_string(height) << " } , 'orientation': { 'x': "<< std::to_string(qx) << " , 'y': "<< std::to_string(qy) << " , 'z': "<< std::to_string(qz) << " , 'w': "<< std::to_string(qw) << " } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, \n"; + ss << "{ 'id': '" << w->name() << "' , 'pos': { 'x': "<< std::to_string(x) << " , 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z + height/2) << " } , 'size': { 'length': "<< std::to_string(length) << " , 'width': "<< std::to_string(width) << " , 'height': "<< std::to_string(height) << " } , 'orientation': { 'x': "<< std::to_string(qx) << " , 'y': "<< std::to_string(qy) << " , 'z': "<< std::to_string(qz) << " , 'w': "<< std::to_string(qw) << " } , 'color': { 'r': 0.15 , 'g': 0.15 , 'b': 0.15 } }, \n"; } @@ -379,7 +383,7 @@ void Mediator::write_file(Robot* A, Robot* B){ float width = w->size().getY(); float height = w->size().getZ(); - ss << "{ 'id': 'table" << B->name().back() << "_" << w->name() << "', 'pos': { 'x': "<< std::to_string(x) << ", 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z + height/2) << " },'size': { 'length': "<< std::to_string(length) << ",'width': "<< std::to_string(width) << ",'height': "<< std::to_string(height) << " },'orientation': { 'x': "<< std::to_string(qx) << ", 'y': "<< std::to_string(qy) << ", 'z': "<< std::to_string(qz) << ", 'w': "<< std::to_string(qw) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, \n"; + ss << "{ 'id': '" << w->name() << "', 'pos': { 'x': "<< std::to_string(x) << ", 'y': "<< std::to_string(y) << " , 'z': "<< std::to_string(z + height/2) << " },'size': { 'length': "<< std::to_string(length) << ",'width': "<< std::to_string(width) << ",'height': "<< std::to_string(height) << " },'orientation': { 'x': "<< std::to_string(qx) << ", 'y': "<< std::to_string(qy) << ", 'z': "<< std::to_string(qz) << ", 'w': "<< std::to_string(qw) << " },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } }, \n"; } tf2::Transform tf_arm = A->tf() * A->root_tf(); float arm_x = tf_arm.getOrigin().getX(); diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp index 75bf7caca2fc030bb358f60948340953e512bc5f..fbdd76144886590e56778662a9d175dce0b9d86d 100644 --- a/src/mtc/src/impl/moveit_mediator.cpp +++ b/src/mtc/src/impl/moveit_mediator.cpp @@ -1,30 +1,40 @@ #include "impl/moveit_mediator.h" -#include "impl/wing_moveit_decorator.h" -#include "impl/collision_helper.h" -#include <moveit/task_constructor/task.h> -#include <moveit/task_constructor/stages/compute_ik.h> -#include <moveit/task_constructor/stages/connect.h> -#include <moveit/task_constructor/stages/current_state.h> -#include <moveit/task_constructor/stages/generate_grasp_pose.h> -#include <moveit/task_constructor/stages/generate_pose.h> -#include <moveit/task_constructor/stages/generate_place_pose.h> -#include <moveit/task_constructor/stages/modify_planning_scene.h> -#include <moveit/task_constructor/stages/move_relative.h> -#include <moveit/task_constructor/stages/move_to.h> -#include <moveit/task_constructor/stages/predicate_filter.h> -#include <moveit/task_constructor/solvers/cartesian_path.h> -#include <moveit/task_constructor/solvers/pipeline_planner.h> -#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h> -#include <eigen_conversions/eigen_msg.h> - - -void Moveit_mediator::publish_tables(moveit::planning_interface::PlanningSceneInterface& psi){ +#include <yaml_to_mtc.h> + + + +void Moveit_mediator::publish_tables(){ ros::WallDuration sleep_time(1.0); for(long unsigned int i = 0; i < robots_.size();i++){ Moveit_robot* ceti = dynamic_cast<Moveit_robot*>(robots_[i]); + std::stringstream ss; + ss << "base_" << ceti->name().back(); + + moveit_msgs::CollisionObject table; + table.id = ss.str(); + table.header.frame_id = "world"; + table.header.stamp = ros::Time::now(); + table.primitives.resize(1); + table.primitives[0].type = table.primitives[0].BOX; + table.primitives[0].dimensions.resize(3); + table.primitives[0].dimensions[0] = ceti->size().getX(); + table.primitives[0].dimensions[1] = ceti->size().getY(); + table.primitives[0].dimensions[2] = ceti->tf().getOrigin().getZ() * 2; + ROS_INFO("%f", table.primitives[0].dimensions[2]); + table.primitive_poses.resize(1); + table.primitive_poses[0].position.x = ceti->tf().getOrigin().getX(); + table.primitive_poses[0].position.y = ceti->tf().getOrigin().getY(); + table.primitive_poses[0].position.z = ceti->tf().getOrigin().getZ(); + table.primitive_poses[0].orientation.x = ceti->tf().getRotation().getX(); + table.primitive_poses[0].orientation.y = ceti->tf().getRotation().getY(); + table.primitive_poses[0].orientation.z = ceti->tf().getRotation().getZ(); + table.primitive_poses[0].orientation.w = ceti->tf().getRotation().getW(); + table.operation = table.ADD; + //psi_->applyCollisionObject(table); + for (Abstract_robot_element* are : ceti->observers()) { Wing_moveit_decorator* wmd = dynamic_cast<Wing_moveit_decorator*>(are); - psi.applyCollisionObject(*wmd->markers()); + psi_->applyCollisionObject(*wmd->markers()); sleep_time.sleep(); } } @@ -43,53 +53,12 @@ bool Moveit_mediator::check_collision(const int& robot){ } void Moveit_mediator::mediate(){ - // create christophs 'bottle' - moveit::planning_interface::PlanningSceneInterface psi; - publish_tables(psi); - - moveit_msgs::CollisionObject bottle; - bottle.id = "bottle"; - bottle.header.frame_id = "world"; - bottle.header.stamp = ros::Time::now(); - bottle.primitives.resize(1); - bottle.primitives[0].type = bottle.primitives[0].BOX; - bottle.primitives[0].dimensions.resize(3); - bottle.primitives[0].dimensions[0] = box_size.getX(); - bottle.primitives[0].dimensions[1] = box_size.getY(); - bottle.primitives[0].dimensions[2] = box_size.getZ(); - - bottle.primitive_poses.resize(1); - bottle.primitive_poses[0].position.x = -0.3f; - bottle.primitive_poses[0].position.y = -0.6f; - bottle.primitive_poses[0].position.z = 0.9305f; - bottle.primitive_poses[0].orientation.x = 0; - bottle.primitive_poses[0].orientation.y = 0; - bottle.primitive_poses[0].orientation.z = 0; - bottle.primitive_poses[0].orientation.w = 1; - bottle.operation = bottle.ADD; - - psi.applyCollisionObject(bottle); - - tf2::Transform target(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.1f, 0.6f, 0.9305f)); - rewrite_task_template(robots_[0], psi.getObjects().at("bottle"), target); - - - /*Yaml_Mtc_Parser parser = Yaml_Mtc_Parser(); - moveit::task_constructor::Task task = parser.init_task(this->nh_); - int max_planning_solutions = 1; // default one solution - this->nh_.getParam("max_planning_solutions", max_planning_solutions); - try { - if (task.plan(max_planning_solutions)){ - moveit_msgs::MoveItErrorCodes execute_result; - - execute_result = task.execute(*task.solutions().front()); - //task.introspection().publishSolution(*task.solutions().front()); - } - } catch (const moveit::task_constructor::InitStageException& ex) { - std::cerr << "planning failed with exception" << std::endl << ex << task; - } + publish_tables(); + setup_task(); + ros::waitForShutdown(); - */ + + }; void Moveit_mediator::build_wings(std::bitset<3>& wing, int& robot){ @@ -113,12 +82,73 @@ void Moveit_mediator::set_wings(std::vector<std::vector<wing_BP>>& wbp){ } }; +void Moveit_mediator::setup_task(){ + ROS_INFO("=> write Task Constructor Objects"); + bool coop = false; + + for(int i = 0; i < objects_.size(); i++){ + if (i+1 < objects_.size()){ + for (long unsigned int j = objects_[i].size()-1; j > 0; j--){ + if(objects_[i][j].getOrigin().distance(objects_[i+1].back().getOrigin()) == 0) { + objects_[i+1].pop_back(); + coop= true; + } + } + } + } -void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target){ - tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z)); - // unique ids cartesian, samplimg, interpolation, ready, hand_open, pick_up + if(coop){ + moveit_msgs::CollisionObject bottle; + bottle.id = "bottle"; + bottle.header.frame_id = "world"; + bottle.header.stamp = ros::Time::now(); + bottle.primitives.resize(1); + bottle.primitives[0].type = bottle.primitives[0].BOX; + bottle.primitives[0].dimensions.resize(3); + bottle.primitives[0].dimensions[0] = box_size.getX(); + bottle.primitives[0].dimensions[1] = box_size.getY(); + bottle.primitives[0].dimensions[2] = box_size.getZ(); + + bottle.primitive_poses.resize(1); + bottle.primitive_poses[0].position.x = -0.3f; + bottle.primitive_poses[0].position.y = -0.6f; + bottle.primitive_poses[0].position.z = 0.9355f; + bottle.primitive_poses[0].orientation.x = 0; + bottle.primitive_poses[0].orientation.y = 0; + bottle.primitive_poses[0].orientation.z = 0; + bottle.primitive_poses[0].orientation.w = 1; + bottle.operation = bottle.ADD; + + psi_->applyCollisionObject(bottle); + + + for( int i = 0; i < objects_.size();i++){ + for(int j =0; j < objects_[i].size();j++){ + create_Task(robots_[i], psi_->getObjects().at("bottle"), objects_[i][j]); + } + } - std::string support_surface1 = "nichts"; + tf2::Transform tr(tf2::Quaternion(0,0,0,1), tf2::Vector3(0, 0, 0.9305f)); + /* + rewrite_task_template(robots_[0], psi_->getObjects().at("bottle"), tr); + + Moveit_robot* mr = dynamic_cast<Moveit_robot*>(robots_[0]); + Yaml_Mtc_Parser parser = Yaml_Mtc_Parser(); + moveit::task_constructor::Task task = parser.init_task(this->nh_); + int max_planning_solutions = 1; // default one solution + this->nh_.getParam("max_planning_solutions", max_planning_solutions); + if (task.plan(max_planning_solutions)){ + moveit_msgs::MoveItErrorCodes execute_result; + execute_result = task.execute(*task.solutions().front()); + } + */ + } + +} + +void Moveit_mediator::create_Task(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target){ + tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z)); + std::string support_surface1 = "nichts"; std::string support_surface2 = "nichts"; @@ -128,125 +158,30 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll if(ar->check_single_object_collision(target, str)) support_surface2= str; } - Moveit_robot* mr = dynamic_cast<Moveit_robot*>(r); - /* - ROS_INFO("%f %f %f source", t.getOrigin().getX(), t.getOrigin().getY(), t.getOrigin().getZ()); - - ROS_INFO("surface1 %s", support_surface1.c_str()); - ROS_INFO("surface2 %s", support_surface2.c_str()); - - - nh_.setParam("/task/properties/group", r->name()); - nh_.setParam("/task/properties/eef", hand); - nh_.setParam("/task/properties/hand_grasping_frame", last_link); - nh_.setParam("/task/properties/ik_frame", last_link); - nh_.setParam("/task/properties/hand", hand); - - XmlRpc::XmlRpcValue a, b, c, d, e, h; - a["group"] = hand; - e["joint_model_group_name"] = hand; - - b = task[4]["stages"]; - b[1]["stage"]["properties"]["object"] = source.id; - b[3]["properties"] = a; - b[2]["set"]["allow_collisions"]["first"] = source.id; - b[2]["set"]["allow_collisions"]["second"] = e; - - c = task[6]["stages"]; - - c[0]["set"]["allow_collisions"]["first"] = source.id; - c[0]["set"]["allow_collisions"]["second"] = support_surface2; - c[3]["properties"] = a; - c[6]["properties"] = a; - c[4]["set"]["allow_collisions"]["first"] = source.id; - c[4]["set"]["allow_collisions"]["second"] = e; - - c[2]["stage"]["set"]["pose"]["point"]["x"]= static_cast<double>(target.getOrigin().getX()); - c[2]["stage"]["set"]["pose"]["point"]["y"]= static_cast<double>(target.getOrigin().getY()); - c[2]["stage"]["set"]["pose"]["point"]["z"]= static_cast<double>(target.getOrigin().getZ()); - + ROS_INFO("ss1 %s", support_surface1.c_str()); + ROS_INFO("ss2 %s", support_surface2.c_str()); - task[2]["properties"] = a; - XmlRpc::XmlRpcValue connect, f, g; - f[r->name()] = "sampling"; - g["source"] = "PARENT"; - connect["type"] = "Connect"; - connect["group_planner_vector"] = f; - connect["propertiesConfigureInitFrom"] = g; - task[5] = connect; - task[3] = connect; - - a.clear(); - e.clear(); - a["link"] = last_link; - a["min_distance"] = 0.07; - a["max_distance"] = 0.2; - c[5]["properties"] = a; - b[0]["properties"] = a; - a["min_distance"] = 0.1; - a["max_distance"] = 0.2; - c[1]["properties"] = a; - - e["object"] = "bottle"; - e["link"] = last_link; - b[4]["set"]["attach_object"] = e; - c[4]["set"]["detach_object"] = e; - c[2]["set"]["ik_frame"]["link"] = last_link; - c[5]["set"]["direction"]["vector"]["header"]["frame_id"] = last_link; - b[1]["set"]["ik_frame"]["link"] = last_link; - b[6]["set"]["ik_frame"]["link"] = last_link; - b[0]["set"]["direction"]["vector"]["header"]["frame_id"] = last_link; - - b[5]["set"]["allow_collisions"]["first"] = source.id; - b[5]["set"]["allow_collisions"]["second"] = support_surface1; - b[7]["set"]["allow_collisions"]["first"] = source.id; - b[7]["set"]["allow_collisions"]["second"] = support_surface1; - - task[6]["stages"] = c; - task[4]["stages"] = b; - - nh_.setParam("task/stages", task); - - */ - const std::string object = source.id; - - // Reset ROS introspection before constructing the new object - // TODO(v4hn): global storage for Introspection services to enable one-liner + const std::string object = source.id; moveit::task_constructor::Task task_; + Moveit_robot* mr = dynamic_cast<Moveit_robot*>(r); - task_.stages()->setName("Pick and Place test"); + std::string name = "Pick&Place"; + task_.stages()->setName(name + std::to_string(static_cast<int>(ros::Time::now().toNSec()))); task_.loadRobotModel(); task_.setRobotModel(mr->mgi()->getRobotModel()); - // Sampling planner - auto sampling_planner = std::make_shared<moveit::task_constructor::solvers::PipelinePlanner>(); - sampling_planner->setProperty("goal_joint_tolerance", 1e-5); - - // Cartesian planner - auto cartesian_planner = std::make_shared<moveit::task_constructor::solvers::CartesianPath>(); - cartesian_planner->setMaxVelocityScaling(1.0); - cartesian_planner->setMaxAccelerationScaling(1.0); - cartesian_planner->setStepSize(.01); - // Set task properties - task_.setProperty("group", "panda_arm1"); - task_.setProperty("eef", "hand_1"); - task_.setProperty("hand", "panda_1_link8"); - task_.setProperty("hand_grasping_frame", "panda_1_link8"); - task_.setProperty("ik_frame", "hand_1"); - - /**************************************************** - * * - * Current State * - * * - ***************************************************/ - moveit::task_constructor::Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator + task_.setProperty("group", mr->name()); + task_.setProperty("eef", mr->map()["eef_name"]); + task_.setProperty("hand", mr->map()["hand_group_name"]); + task_.setProperty("hand_grasping_frame", mr->map()["hand_frame"]); + task_.setProperty("ik_frame", mr->map()["hand_frame"]); + + moveit::task_constructor::Stage* current_state_ptr = nullptr; { auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>("current state"); - - // Verify that object is not attached auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>("applicability test", std::move(current_state)); applicability_filter->setPredicate([object](const moveit::task_constructor::SolutionBase& s, std::string& comment) { if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) { @@ -260,63 +195,43 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll task_.add(std::move(applicability_filter)); } - /**************************************************** - * * - * Open Hand * - * * - ***************************************************/ { // Open Hand - auto stage = std::make_unique< moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner); - stage->setGroup("hand_1"); + auto stage = std::make_unique< moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_); + stage->setGroup(mr->map()["eef_name"]); stage->setGoal("open"); task_.add(std::move(stage)); } - /**************************************************** - * * - * Move to Pick * - * * - ***************************************************/ { // Move-to pre-grasp auto stage = std::make_unique< moveit::task_constructor::stages::Connect>( - "move to pick", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { "panda_arm1", sampling_planner} }); + "move to pick", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(), sampling_planner_} }); stage->setTimeout(5.0); stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT); task_.add(std::move(stage)); } - /**************************************************** - * * - * Pick Object * - * * - ***************************************************/ moveit::task_constructor::Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator { auto grasp = std::make_unique<moveit::task_constructor::SerialContainer>("pick object"); task_.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); grasp->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); - /**************************************************** - ---- * Approach Object * - ***************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("approach object", cartesian_planner); + + { // Approach Obj + auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("approach object", cartesian_planner_); stage->properties().set("marker_ns", "approach_object"); - stage->properties().set("link", "panda_1_link8"); + stage->properties().set("link", mr->map()["hand_frame"]); stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" }); stage->setMinMaxDistance(0.07, 0.2); // Set hand forward direction geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = "panda_1_link8"; + vec.header.frame_id = mr->map()["hand_frame"]; vec.vector.z = 1.0; stage->setDirection(vec); grasp->insert(std::move(stage)); } - /**************************************************** - ---- * Generate Grasp Pose * - ***************************************************/ { // Sample grasp pose auto stage = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate grasp pose"); @@ -324,7 +239,7 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll stage->properties().set("marker_ns", "grasp_pose"); stage->setPreGraspPose("open"); stage->setObject(object); - stage->setAngleDelta(M_PI / 2); + stage->setAngleDelta(M_PI / 12); stage->setMonitoredStage(current_state_ptr); // Hook into current state // Compute IK @@ -337,60 +252,45 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("grasp pose IK", std::move(stage)); wrapper->setMaxIKSolutions(8); wrapper->setMinSolutionDistance(1.0); - wrapper->setIKFrame(ik, "panda_1_link8"); + wrapper->setIKFrame(ik, mr->map()["hand_frame"]); wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" }); wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" }); grasp->insert(std::move(wrapper)); } - /**************************************************** - ---- * Allow Collision (hand object) * - ***************************************************/ { auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)"); stage->allowCollisions( - object, task_.getRobotModel()->getJointModelGroup("hand_1")->getLinkModelNamesWithCollisionGeometry(), + object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(), true); grasp->insert(std::move(stage)); } - /**************************************************** - ---- * Close Hand * - ***************************************************/ { - auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner); - stage->setGroup("hand_1"); + auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner_); + stage->setGroup(mr->map()["eef_name"]); stage->setGoal("close"); grasp->insert(std::move(stage)); } - /**************************************************** - .... * Attach Object * - ***************************************************/ - { + { // Attach obj auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("attach object"); - stage->attachObject(object, "panda_1_link8"); + stage->attachObject(object, mr->map()["hand_frame"]); attach_object_stage = stage.get(); grasp->insert(std::move(stage)); } - /**************************************************** - .... * Allow collision (object support) * - ***************************************************/ - { + { // Allow Collision obj table auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (object,support)"); stage->allowCollisions({ object }, support_surface1, true); grasp->insert(std::move(stage)); } - /**************************************************** - .... * Lift object * - ***************************************************/ { - auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lift object", cartesian_planner); + auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lift object", cartesian_planner_); stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" }); stage->setMinMaxDistance(0.1, 0.2); - stage->setIKFrame("panda_1_link8"); + stage->setIKFrame(mr->map()["hand_frame"]); stage->properties().set("marker_ns", "lift_object"); // Set upward direction @@ -401,10 +301,7 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll grasp->insert(std::move(stage)); } - /**************************************************** - .... * Forbid collision (object support) * - ***************************************************/ - { + { // forbid collision auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)"); stage->allowCollisions({ object }, support_surface1, false); grasp->insert(std::move(stage)); @@ -414,45 +311,30 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll task_.add(std::move(grasp)); } - /****************************************************** - * * - * Move to Place * - * * - *****************************************************/ { auto stage = std::make_unique<moveit::task_constructor::stages::Connect>( - "move to place", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { "panda_arm1", sampling_planner } }); + "move to place", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { mr->name(), sampling_planner_ } }); stage->setTimeout(5.0); stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT); task_.add(std::move(stage)); } - /****************************************************** - * * - * Place Object * - * * - *****************************************************/ { auto place = std::make_unique<moveit::task_constructor::SerialContainer>("place object"); task_.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); place->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group" }); - /**************************************************** - .... * Forbid collision (object support) * - ***************************************************/ { - auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)"); - stage->allowCollisions({ object }, support_surface2, true); - place->insert(std::move(stage)); + auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow cokbkmomsurface)"); + stage->allowCollisions( {object} , support_surface2, true); + place->insert(std::move(stage)); } - /****************************************************** - ---- * Lower Object * - *****************************************************/ + { - auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lower object", cartesian_planner); + auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lower object", cartesian_planner_); stage->properties().set("marker_ns", "lower_object"); - stage->properties().set("link", "panda_1_link8"); + stage->properties().set("link", mr->map()["hand_frame"]); stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" }); stage->setMinMaxDistance(.03, .13); @@ -464,9 +346,6 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll place->insert(std::move(stage)); } - /****************************************************** - ---- * Generate Place Pose * - *****************************************************/ { // Generate Place Pose auto stage = std::make_unique<moveit::task_constructor::stages::GeneratePlacePose>("generate place pose"); @@ -479,7 +358,7 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll p.header.frame_id = "world"; p.pose.position.x = target.getOrigin().getX(); p.pose.position.y = target.getOrigin().getY(); - p.pose.position.z = target.getOrigin().getZ(); + p.pose.position.z = 0.9355f; p.pose.orientation.x = target.getRotation().getX(); p.pose.orientation.y = target.getRotation().getY(); p.pose.orientation.z = target.getRotation().getZ(); @@ -498,53 +377,41 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll Eigen::Isometry3d ik = eigen * trans; auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("place pose IK", std::move(stage)); wrapper->setMaxIKSolutions(2); - wrapper->setIKFrame(ik, "panda_1_link8"); + wrapper->setIKFrame(ik, mr->map()["hand_frame"]); wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" }); wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" }); place->insert(std::move(wrapper)); } - /****************************************************** - ---- * Open Hand * - *****************************************************/ { - auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner); - stage->setGroup("hand_1"); + auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner_); + stage->setGroup(mr->map()["eef_name"]); stage->setGoal("open"); place->insert(std::move(stage)); } - /****************************************************** - ---- * Forbid collision (hand, object) * - *****************************************************/ { auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)"); stage->allowCollisions( - object, task_.getRobotModel()->getJointModelGroup("hand_1")->getLinkModelNamesWithCollisionGeometry(), + object, task_.getRobotModel()->getJointModelGroup(mr->map()["eef_name"])->getLinkModelNamesWithCollisionGeometry(), false); place->insert(std::move(stage)); } - /****************************************************** - ---- * Detach Object * - *****************************************************/ { auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("detach object"); - stage->detachObject(object, "panda_1_link8"); + stage->detachObject(object, mr->map()["hand_frame"]); place->insert(std::move(stage)); } - /****************************************************** - ---- * Retreat Motion * - *****************************************************/ { - auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("retreat after place", cartesian_planner); + auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("retreat after place", cartesian_planner_); stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" }); - stage->setMinMaxDistance(.12, .25); - stage->setIKFrame("panda_1_link8"); + stage->setMinMaxDistance(.1, .2); + stage->setIKFrame(mr->map()["hand_frame"]); stage->properties().set("marker_ns", "retreat"); geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = "panda_1_link8"; + vec.header.frame_id = mr->map()["hand_frame"]; vec.vector.z = -1.0; stage->setDirection(vec); place->insert(std::move(stage)); @@ -554,387 +421,122 @@ void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::Coll task_.add(std::move(place)); } - /****************************************************** - * * - * Move to Home * - * * - *****************************************************/ { - auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("move home", sampling_planner); + auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("move home", sampling_planner_); stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" }); stage->setGoal("ready"); stage->restrictDirection(moveit::task_constructor::stages::MoveTo::FORWARD); task_.add(std::move(stage)); } - - // ------------------ - Moveit_robot* mr2 = dynamic_cast<Moveit_robot*>(robots_[1]); - moveit::task_constructor::Task task; + //task_map_.at(mr->name()).push_back(std::move(task_)); - - task.stages()->setName("Pick and Place test1"); - task.loadRobotModel(); - task.setRobotModel(mr2->mgi()->getRobotModel()); - - - - // Set task properties - task.setProperty("group", "panda_arm2"); - task.setProperty("eef", "hand_2"); - task.setProperty("hand", "panda_2_link8"); - task.setProperty("hand_grasping_frame", "panda_2_link8"); - task.setProperty("ik_frame", "hand_2"); - - /**************************************************** - * * - * Current State * - * * - ***************************************************/ - current_state_ptr = nullptr; // Forward current_state on to grasp pose generator - { - auto current_state = std::make_unique< moveit::task_constructor::stages::CurrentState>("current state"); - - // Verify that object is not attached - auto applicability_filter = std::make_unique< moveit::task_constructor::stages::PredicateFilter>("applicability test", std::move(current_state)); - applicability_filter->setPredicate([object](const moveit::task_constructor::SolutionBase& s, std::string& comment) { - if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) { - comment = "object with id '" + object + "' is already attached and cannot be picked"; - return false; - } - return true; - }); - - current_state_ptr = applicability_filter.get(); - task.add(std::move(applicability_filter)); - } - - /**************************************************** - * * - * Open Hand * - * * - ***************************************************/ - { // Open Hand - auto stage = std::make_unique< moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner); - stage->setGroup("hand_2"); - stage->setGoal("open"); - task.add(std::move(stage)); + if (task_.plan(1)){ + moveit_msgs::MoveItErrorCodes execute_result; + execute_result = task_.execute(*task_.solutions().front()); + //task_.introspection().publishSolution(*task_.solutions().front()); } - /**************************************************** - * * - * Move to Pick * - * * - ***************************************************/ - { // Move-to pre-grasp - auto stage = std::make_unique< moveit::task_constructor::stages::Connect>( - "move to pick", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { "panda_arm2", sampling_planner} }); - stage->setTimeout(5.0); - stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT); - task.add(std::move(stage)); - } - - /**************************************************** - * * - * Pick Object * - * * - ***************************************************/ - attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator - { - auto grasp = std::make_unique<moveit::task_constructor::SerialContainer>("pick object"); - task.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); - grasp->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); - - /**************************************************** - ---- * Approach Object * - ***************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("approach object", cartesian_planner); - stage->properties().set("marker_ns", "approach_object"); - stage->properties().set("link", "panda_2_link8"); - stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" }); - stage->setMinMaxDistance(0.07, 0.2); - - // Set hand forward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = "panda_2_link8"; - vec.vector.z = 1.0; - stage->setDirection(vec); - grasp->insert(std::move(stage)); - } - - /**************************************************** - ---- * Generate Grasp Pose * - ***************************************************/ - { - // Sample grasp pose - auto stage = std::make_unique<moveit::task_constructor::stages::GenerateGraspPose>("generate grasp pose"); - stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT); - stage->properties().set("marker_ns", "grasp_pose"); - stage->setPreGraspPose("open"); - stage->setObject(object); - stage->setAngleDelta(M_PI / 2); - stage->setMonitoredStage(current_state_ptr); // Hook into current state - - // Compute IK - Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ()); - Eigen::Translation3d trans(0.1f, 0, 0); - Eigen::Isometry3d ik = eigen * trans; - - auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("grasp pose IK", std::move(stage)); - wrapper->setMaxIKSolutions(8); - wrapper->setMinSolutionDistance(1.0); - wrapper->setIKFrame(ik, "panda_2_link8"); - wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" }); - wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" }); - grasp->insert(std::move(wrapper)); - } - - /**************************************************** - ---- * Allow Collision (hand object) * - ***************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)"); - stage->allowCollisions( - object, task.getRobotModel()->getJointModelGroup("hand_2")->getLinkModelNamesWithCollisionGeometry(), - true); - grasp->insert(std::move(stage)); - } - - /**************************************************** - ---- * Close Hand * - ***************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("close hand", sampling_planner); - stage->setGroup("hand_2"); - stage->setGoal("close"); - grasp->insert(std::move(stage)); - } - - /**************************************************** - .... * Attach Object * - ***************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("attach object"); - stage->attachObject(object, "panda_2_link8"); - attach_object_stage = stage.get(); - grasp->insert(std::move(stage)); - } - - /**************************************************** - .... * Allow collision (object support) * - ***************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (object,support)"); - stage->allowCollisions({ object }, support_surface1, true); - grasp->insert(std::move(stage)); - } - - /**************************************************** - .... * Lift object * - ***************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lift object", cartesian_planner); - stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" }); - stage->setMinMaxDistance(0.1, 0.2); - stage->setIKFrame("panda_2_link8"); - stage->properties().set("marker_ns", "lift_object"); - - // Set upward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = "world"; - vec.vector.z = 1.0; - stage->setDirection(vec); - grasp->insert(std::move(stage)); - } +} - /**************************************************** - .... * Forbid collision (object support) * - ***************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)"); - stage->allowCollisions({ object }, support_surface1, false); - grasp->insert(std::move(stage)); - } - // Add grasp container to task - task.add(std::move(grasp)); - } +void Moveit_mediator::rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target){ + tf2::Transform t(tf2::Quaternion(source.pose.orientation.x, source.pose.orientation.y, source.pose.orientation.z, source.pose.orientation.w), tf2::Vector3(source.pose.position.x, source.pose.position.y, source.pose.position.z)); + // unique ids cartesian, samplimg, interpolation, ready, hand_open, pick_up - /****************************************************** - * * - * Move to Place * - * * - *****************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::Connect>( - "move to place", moveit::task_constructor::stages::Connect::GroupPlannerVector{ { "panda_arm2", sampling_planner } }); - stage->setTimeout(5.0); - stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT); - task.add(std::move(stage)); - } + std::string support_surface1 = "nichts"; + std::string support_surface2 = "nichts"; - /****************************************************** - * * - * Place Object * - * * - *****************************************************/ - { - auto place = std::make_unique<moveit::task_constructor::SerialContainer>("place object"); - task.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); - place->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "hand", "group" }); + XmlRpc::XmlRpcValue task; + nh_.getParam("task/stages", task); - /**************************************************** - .... * Forbid collision (object support) * - ***************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("forbid collision (object,surface)"); - stage->allowCollisions({ object }, support_surface2, true); - place->insert(std::move(stage)); - } - /****************************************************** - ---- * Lower Object * - *****************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("lower object", cartesian_planner); - stage->properties().set("marker_ns", "lower_object"); - stage->properties().set("link", "panda_2_link8"); - stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" }); - stage->setMinMaxDistance(.03, .13); - // Set downward direction - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = "world"; - vec.vector.z = -1.0; - stage->setDirection(vec); - place->insert(std::move(stage)); - } + for (Abstract_robot* ar : robots_){ + std::string str; + if(ar->check_single_object_collision(t, str)) support_surface1 = str; + if(ar->check_single_object_collision(target, str)) support_surface2= str; + } - /****************************************************** - ---- * Generate Place Pose * - *****************************************************/ - { - // Generate Place Pose - auto stage = std::make_unique<moveit::task_constructor::stages::GeneratePlacePose>("generate place pose"); - stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "ik_frame" }); - stage->properties().set("marker_ns", "place_pose"); - stage->setObject(object); + Moveit_robot* mr = dynamic_cast<Moveit_robot*>(r); + + ROS_INFO("%f %f %f source", t.getOrigin().getX(), t.getOrigin().getY(), t.getOrigin().getZ()); - // Set target pose - geometry_msgs::PoseStamped p; - p.header.frame_id = "world"; - p.pose.position.x = target.getOrigin().getX(); - p.pose.position.y = target.getOrigin().getY(); - p.pose.position.z = target.getOrigin().getZ(); - p.pose.orientation.x = target.getRotation().getX(); - p.pose.orientation.y = target.getRotation().getY(); - p.pose.orientation.z = target.getRotation().getZ(); - p.pose.orientation.w = target.getRotation().getW(); + ROS_INFO("surface1 %s", support_surface1.c_str()); + ROS_INFO("surface2 %s", support_surface2.c_str()); + nh_.setParam("/task/properties/group", r->name()); + nh_.setParam("/task/properties/eef", mr->map()["eef"]); + nh_.setParam("/task/properties/hand_grasping_frame", mr->map()["hand_grasping_frame"]); + nh_.setParam("/task/properties/ik_frame", mr->map()["ik_frame"]); + nh_.setParam("/task/properties/hand", mr->map()["hand"]); - stage->setPose(p); - stage->setMonitoredStage(attach_object_stage); // Hook into attach_object_stage + XmlRpc::XmlRpcValue a, b, c, d, e, h; + a["group"] = mr->map()["hand"]; + e["joint_model_group_name"] = mr->map()["hand"]; - // Compute IK - Eigen::Quaterniond eigen = Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(0.785f, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(1.571f, Eigen::Vector3d::UnitZ()); - Eigen::Translation3d trans(0.1f, 0, 0); - Eigen::Isometry3d ik = eigen * trans; - auto wrapper = std::make_unique<moveit::task_constructor::stages::ComputeIK>("place pose IK", std::move(stage)); - wrapper->setMaxIKSolutions(2); - wrapper->setIKFrame(ik, "panda_2_link8"); - wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "eef", "group" }); - wrapper->properties().configureInitFrom(moveit::task_constructor::Stage::INTERFACE, { "target_pose" }); - place->insert(std::move(wrapper)); - } + b = task[4]["stages"]; + b[1]["stage"]["properties"]["object"] = source.id; + b[3]["properties"] = a; + b[2]["set"]["allow_collisions"]["first"] = source.id; + b[2]["set"]["allow_collisions"]["second"] = e; - /****************************************************** - ---- * Open Hand * - *****************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("open hand", sampling_planner); - stage->setGroup("hand_2"); - stage->setGoal("open"); - place->insert(std::move(stage)); - } + c = task[6]["stages"]; - /****************************************************** - ---- * Forbid collision (hand, object) * - *****************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("allow collision (hand,object)"); - stage->allowCollisions( - object, task.getRobotModel()->getJointModelGroup("hand_2")->getLinkModelNamesWithCollisionGeometry(), - false); - place->insert(std::move(stage)); - } + c[0]["set"]["allow_collisions"]["first"] = source.id; + c[0]["set"]["allow_collisions"]["second"] = support_surface2; + c[3]["properties"] = a; + c[6]["properties"] = a; + c[4]["set"]["allow_collisions"]["first"] = source.id; + c[4]["set"]["allow_collisions"]["second"] = e; - /****************************************************** - ---- * Detach Object * - *****************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("detach object"); - stage->detachObject(object, "panda_2_link8"); - place->insert(std::move(stage)); - } + c[2]["stage"]["set"]["pose"]["point"]["x"]= static_cast<double>(target.getOrigin().getX()); + c[2]["stage"]["set"]["pose"]["point"]["y"]= static_cast<double>(target.getOrigin().getY()); + c[2]["stage"]["set"]["pose"]["point"]["z"]= static_cast<double>(target.getOrigin().getZ()); - /****************************************************** - ---- * Retreat Motion * - *****************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::MoveRelative>("retreat after place", cartesian_planner); - stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" }); - stage->setMinMaxDistance(.12, .25); - stage->setIKFrame("panda_2_link8"); - stage->properties().set("marker_ns", "retreat"); - geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = "panda_2_link8"; - vec.vector.z = -1.0; - stage->setDirection(vec); - place->insert(std::move(stage)); - } - // Add place container to task - task.add(std::move(place)); - } + task[2]["properties"] = a; - /****************************************************** - * * - * Move to Home * - * * - *****************************************************/ - { - auto stage = std::make_unique<moveit::task_constructor::stages::MoveTo>("move home", sampling_planner); - stage->properties().configureInitFrom(moveit::task_constructor::Stage::PARENT, { "group" }); - stage->setGoal("ready"); - stage->restrictDirection(moveit::task_constructor::stages::MoveTo::FORWARD); - task.add(std::move(stage)); - } + XmlRpc::XmlRpcValue connect, f, g; + f[r->name()] = "sampling"; + g["source"] = "PARENT"; + connect["type"] = "Connect"; + connect["group_planner_vector"] = f; + connect["propertiesConfigureInitFrom"] = g; + task[5] = connect; + task[3] = connect; - int max_planning_solution = 1; - if (task_.plan(max_planning_solution)){ - moveit_msgs::MoveItErrorCodes execute_result; + a.clear(); + e.clear(); + a["link"] = mr->map()["ik_frame"]; + a["min_distance"] = 0.07; + a["max_distance"] = 0.2; + c[5]["properties"] = a; + b[0]["properties"] = a; + a["min_distance"] = 0.1; + a["max_distance"] = 0.2; + c[1]["properties"] = a; - execute_result = task_.execute(*task_.solutions().front()); - //task_.introspection().publishSolution(*task_.solutions().front()); - } + e["object"] = "bottle"; + e["link"] = mr->map()["ik_frame"]; + b[4]["set"]["attach_object"] = e; + c[4]["set"]["detach_object"] = e; + c[2]["set"]["ik_frame"]["link"] = mr->map()["ik_frame"]; + c[5]["set"]["direction"]["vector"]["header"]["frame_id"] = mr->map()["ik_frame"]; + b[1]["set"]["ik_frame"]["link"] = mr->map()["ik_frame"]; + b[6]["set"]["ik_frame"]["link"] = mr->map()["ik_frame"]; + b[0]["set"]["direction"]["vector"]["header"]["frame_id"] = mr->map()["ik_frame"]; + b[5]["set"]["allow_collisions"]["first"] = source.id; + b[5]["set"]["allow_collisions"]["second"] = support_surface1; + b[7]["set"]["allow_collisions"]["first"] = source.id; + b[7]["set"]["allow_collisions"]["second"] = support_surface1; - if (task.plan(max_planning_solution)){ - moveit_msgs::MoveItErrorCodes execute_result; + task[6]["stages"] = c; + task[4]["stages"] = b; - execute_result = task.execute(*task.solutions().front()); - //task_.introspection().publishSolution(*task_.solutions().front()); - } + nh_.setParam("task/stages", task); } - -void Moveit_mediator::setup_task (){ - -} diff --git a/src/mtc/src/impl/wing_moveit_decorator.cpp b/src/mtc/src/impl/wing_moveit_decorator.cpp index e026b132b48f1c4652a97baa75d67fbed85214df..6f71327624bfc221cd31367109c9c133a615f7be 100644 --- a/src/mtc/src/impl/wing_moveit_decorator.cpp +++ b/src/mtc/src/impl/wing_moveit_decorator.cpp @@ -27,7 +27,7 @@ void Wing_moveit_decorator::output_filter() { markers_->primitive_poses.resize(1); markers_->primitive_poses[0].position.x = world_origin.getX(); markers_->primitive_poses[0].position.y = world_origin.getY(); - markers_->primitive_poses[0].position.z = world_origin.getZ(); + markers_->primitive_poses[0].position.z = world_origin.getZ() - wing->size().getZ()/2; markers_->primitive_poses[0].orientation.x = world_quat.getX(); markers_->primitive_poses[0].orientation.y = world_quat.getY(); markers_->primitive_poses[0].orientation.z = world_quat.getZ(); diff --git a/src/mtc/src/two_finger_grasp_data.cpp b/src/mtc/src/two_finger_grasp_data.cpp deleted file mode 100644 index f2db2eb0d9fc5891f89069a42a6f5798917f034c..0000000000000000000000000000000000000000 --- a/src/mtc/src/two_finger_grasp_data.cpp +++ /dev/null @@ -1,215 +0,0 @@ -/* - * Software License Agreement (Modified BSD License) - * - * Copyright (c) 2014, University of Colorado, Boulder, PAL Robotics, S.L. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Univ of CO, Boulder, PAL Robotics, S.L. - * nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific - * prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -/* Authors: Dave Coleman, Bence Magyar - Description: Data class used by the grasp generator. -*/ - -#include "../include/two_finger_grasp_data.h" - -// Eigen -#include <Eigen/Core> -#include <Eigen/Geometry> -#include <eigen_conversions/eigen_msg.h> - -// C++ -#include <cmath> - -// Parameter loading -#include <rosparam_shortcuts/rosparam_shortcuts.h> - -// Pose conversion -#include <rviz_visual_tools/rviz_visual_tools.h> - -namespace moveit_grasps -{ -const std::string LOGNAME = "grasp_data.two_finger_gripper"; - -TwoFingerGraspData::TwoFingerGraspData(const ros::NodeHandle& nh, const std::string& end_effector, - const moveit::core::RobotModelConstPtr& robot_model) - : GraspData(nh, end_effector, robot_model) -{ -} - -bool TwoFingerGraspData::loadGraspData(const ros::NodeHandle& nh, const std::string& end_effector) -{ - if (!GraspData::loadGraspData(nh, end_effector)) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "GraspData::loadGraspData failed"); - return false; - } - // Load all other parameters - const std::string parent_name = "grasp_data"; // for namespacing logging messages - std::size_t error = 0; - - // Search within the sub-namespace of this end effector name - ros::NodeHandle child_nh(nh, end_effector); - - error += !rosparam_shortcuts::get(parent_name, child_nh, "gripper_finger_width", gripper_finger_width_); - error += !rosparam_shortcuts::get(parent_name, child_nh, "max_grasp_width", max_grasp_width_); - error += !rosparam_shortcuts::get(parent_name, child_nh, "max_finger_width", max_finger_width_); - error += !rosparam_shortcuts::get(parent_name, child_nh, "min_finger_width", min_finger_width_); - rosparam_shortcuts::shutdownIfError(parent_name, error); - - return true; -} - -bool TwoFingerGraspData::setGraspWidth(double fraction_open, double min_finger_width, - trajectory_msgs::JointTrajectory& grasp_posture) -{ - if (fraction_open < 0 || fraction_open > 1) - { - ROS_ERROR_STREAM_NAMED("grasp_data", "Invalid fraction passed in " << fraction_open); - return false; - } - - // Ensure min_finger_width is not less than actual min finger width - double min_finger_width_adjusted = std::max(min_finger_width, min_finger_width_); - - // Max width = max_finger_width_ - // Min width = min_finger_width_adjusted - double distance_btw_fingers = - min_finger_width_adjusted + (max_finger_width_ - min_finger_width_adjusted) * fraction_open; - return fingerWidthToGraspPosture(distance_btw_fingers, grasp_posture); -} - -bool TwoFingerGraspData::fingerWidthToGraspPosture(double distance_btw_fingers, - trajectory_msgs::JointTrajectory& grasp_posture) -{ - // TODO(mlautman): Change this function to take in a method for translating joint values to grasp width - // Currently this function simply interpolates between max open and max closed - ROS_DEBUG_STREAM_NAMED("grasp_data", - "Setting grasp posture to have distance_between_fingers of " << distance_btw_fingers); - - // Error check - if (distance_btw_fingers > max_finger_width_ + std::numeric_limits<double>::epsilon() || - distance_btw_fingers < min_finger_width_ - std::numeric_limits<double>::epsilon()) - { - ROS_DEBUG_STREAM_NAMED("grasp_data", "Requested " << distance_btw_fingers << " is beyond limits of " - << min_finger_width_ << "," << max_finger_width_); - return false; - } - - // NOTE: We assume a linear relationship between the actuated joint values and the distance between fingers. - // This is probably incorrect but until we expose an interface for passing in a function to translate from - // joint values to grasp width, it's the best we got... - // TODO(mlautman): Make it so that a user can pass in a function here. - const std::vector<std::string>& joint_names = pre_grasp_posture_.joint_names; - const std::vector<double>& grasp_pose = grasp_posture_.points[0].positions; - const std::vector<double>& pre_grasp_pose = pre_grasp_posture_.points[0].positions; - if (joint_names.size() != grasp_pose.size() || grasp_pose.size() != pre_grasp_pose.size()) - { - ROS_ERROR_NAMED("grasp_data", - "Mismatched vector sizes joint_names.size()=%zu, grasp_pose.size()=%zu, and " - "pre_grasp_pose.size()=%zu", - joint_names.size(), grasp_pose.size(), pre_grasp_pose.size()); - return false; - } - - std::vector<double> slope(joint_names.size()); - std::vector<double> intercept(joint_names.size()); - std::vector<double> joint_positions(joint_names.size()); - for (std::size_t joint_index = 0; joint_index < joint_names.size(); joint_index++) - { - slope[joint_index] = - (max_finger_width_ - min_finger_width_) / (pre_grasp_pose[joint_index] - grasp_pose[joint_index]); - intercept[joint_index] = max_finger_width_ - slope[joint_index] * pre_grasp_pose[joint_index]; - - // Sanity check - ROS_ASSERT_MSG(intercept[joint_index] == min_finger_width_ - slope[joint_index] * grasp_pose[joint_index], - "we got different y intercept!! %.3f and %.3f", intercept[joint_index], - min_finger_width_ - slope[joint_index] * grasp_pose[joint_index]); - - joint_positions[joint_index] = (distance_btw_fingers - intercept[joint_index]) / slope[joint_index]; - - ROS_DEBUG_NAMED("grasp_data", "Converted joint %s to position %.3f", joint_names[joint_index].c_str(), - joint_positions[joint_index]); - } - - return jointPositionsToGraspPosture(joint_positions, grasp_posture); -} - -bool TwoFingerGraspData::jointPositionsToGraspPosture(const std::vector<double>& joint_positions, - trajectory_msgs::JointTrajectory& grasp_posture) -{ - for (std::size_t joint_index = 0; joint_index < pre_grasp_posture_.joint_names.size(); joint_index++) - { - const moveit::core::JointModel* joint = robot_model_->getJointModel(pre_grasp_posture_.joint_names[joint_index]); - ROS_ASSERT_MSG(joint->getVariableBounds().size() > 0, "joint->getVariableBounds() is empty for %s", - pre_grasp_posture_.joint_names[joint_index].c_str()); - const moveit::core::VariableBounds& bound = joint->getVariableBounds()[0]; - - if (joint_positions[joint_index] > bound.max_position_ || joint_positions[joint_index] < bound.min_position_) - { - ROS_ERROR_STREAM_NAMED("grasp_data", "Requested joint " << pre_grasp_posture_.joint_names[joint_index].c_str() - << "at index" << joint_index << " with value " - << joint_positions[joint_index] << " is beyond limits of " - << bound.min_position_ << ", " << bound.max_position_); - return false; - } - } - - // Get default grasp posture - grasp_posture = grasp_posture_; - - // Error check - if (joint_positions.size() != grasp_posture.points.front().positions.size()) - { - ROS_ERROR_STREAM_NAMED("grasp_data", - "Not enough finger joints passed in: " << joint_positions.size() << " positions but expect " - << grasp_posture.points.front().positions.size()); - return false; - } - - // Set joint positions - grasp_posture.points.front().positions = joint_positions; - - return true; -} - -void TwoFingerGraspData::print() -{ - GraspData::print(); - - std::cout << "Finger Gripper Parameters: " << std::endl; - std::cout << "\tpre_grasp_posture_: \n" << pre_grasp_posture_ << std::endl; - std::cout << "\tgrasp_posture_: \n" << grasp_posture_ << std::endl; - std::cout << "\tgripper_finger_width_: " << gripper_finger_width_ << std::endl; - std::cout << "\tmax_grasp_width_: " << max_grasp_width_ << std::endl; - std::cout << "\tmax_finger_width_: " << max_finger_width_ << std::endl; - std::cout << "\tmin_finger_width_: " << min_finger_width_ << std::endl; -} - -} // namespace moveit_grasps diff --git a/src/mtc/src/two_finger_grasp_filter.cpp b/src/mtc/src/two_finger_grasp_filter.cpp deleted file mode 100644 index 11d2376983c8368e135441d014e061f919c1f681..0000000000000000000000000000000000000000 --- a/src/mtc/src/two_finger_grasp_filter.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman <dave@picknik.ai> - Desc: Filters grasps based on kinematic feasibility and collision -*/ - -// Parent Class -#include "../include/state_validity_callback.h" - -// moveit_grasps -#include "../include/two_finger_grasp_filter.h" - -// moveit -#include <moveit/transforms/transforms.h> -#include <moveit/collision_detection/collision_tools.h> - -namespace moveit_grasps -{ -// Constructor -TwoFingerGraspFilter::TwoFingerGraspFilter(const robot_state::RobotStatePtr& robot_state, - const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools) - : GraspFilter(robot_state, visual_tools), name_("two_finger_grasp_filter") -{ -} - -bool TwoFingerGraspFilter::processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct) -{ - bool filer_results = GraspFilter::processCandidateGrasp(ik_thread_struct); - if (!filer_results) - return false; - - // Helper pointer - GraspCandidatePtr& grasp_candidate = ik_thread_struct->grasp_candidates_[ik_thread_struct->grasp_id]; - - moveit::core::GroupStateValidityCallbackFn constraint_fn = - boost::bind(&isGraspStateValid, ik_thread_struct->planning_scene_.get(), - collision_verbose_ || ik_thread_struct->visual_debug_, collision_verbose_speed_, visual_tools_, _1, - _2, _3); - - // Check if IK solution for grasp pose is valid for fingers closed as well - return checkFingersClosedIK(grasp_candidate->grasp_ik_solution_, ik_thread_struct, grasp_candidate, constraint_fn); -} - -bool TwoFingerGraspFilter::checkFingersClosedIK(std::vector<double>& ik_solution, - const IkThreadStructPtr& ik_thread_struct, - GraspCandidatePtr& grasp_candidate, - const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const -{ - // Set gripper position (how open the fingers are) to CLOSED - grasp_candidate->getGraspStateClosedEEOnly(ik_thread_struct->robot_state_); - - // Check constraint function - if (!constraint_fn(ik_thread_struct->robot_state_.get(), grasp_candidate->grasp_data_->arm_jmg_, &ik_solution[0])) - { - ROS_WARN_STREAM_NAMED(name_ + ".superdebug", "Grasp filtered because in collision with fingers CLOSED"); - grasp_candidate->grasp_filtered_code_ = GraspFilterCode::GRASP_FILTERED_BY_IK_CLOSED; - return false; - } - - return true; -} - -} // namespace moveit_grasps diff --git a/src/mtc/src/two_finger_grasp_generator.cpp b/src/mtc/src/two_finger_grasp_generator.cpp deleted file mode 100644 index 969967742f554dab2d52d00c465ed10a9688a85d..0000000000000000000000000000000000000000 --- a/src/mtc/src/two_finger_grasp_generator.cpp +++ /dev/null @@ -1,887 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2015, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman <dave@picknik.ai>, Andy McEvoy - Desc: Generates geometric grasps for cuboids and blocks, not using physics or contact wrenches -*/ - -#include "../include/two_finger_grasp_generator.h" -#include "../include/grasp_filter.h" - -#include <rosparam_shortcuts/rosparam_shortcuts.h> - -namespace -{ -void debugFailedOpenGripper(double percent_open, double min_finger_open_on_approach, double object_width, - double grasp_padding_on_approach) -{ - ROS_ERROR_STREAM_NAMED("grasp_generator", "Unable to set grasp width to " - << percent_open << " % open. Stats:" - << "\n min_finger_open_on_approach: \t " << min_finger_open_on_approach - << "\n object_width: \t " << object_width - << "\n grasp_padding_on_approach_: \t " << grasp_padding_on_approach); -} - -} // namespace - -namespace moveit_grasps -{ -TwoFingerGraspCandidateConfig::TwoFingerGraspCandidateConfig() - : enable_corner_grasps_(true) - , enable_face_grasps_(true) - , enable_variable_angle_grasps_(true) - , enable_edge_grasps_(true) - , generate_x_axis_grasps_(true) - , generate_y_axis_grasps_(true) - , generate_z_axis_grasps_(true) -{ -} -void TwoFingerGraspCandidateConfig::enableAllGraspTypes() -{ - enable_corner_grasps_ = true; - enable_face_grasps_ = true; - enable_variable_angle_grasps_ = true; - enable_edge_grasps_ = true; -} -void TwoFingerGraspCandidateConfig::enableAllGraspAxes() -{ - generate_x_axis_grasps_ = true; - generate_y_axis_grasps_ = true; - generate_z_axis_grasps_ = true; -} -void TwoFingerGraspCandidateConfig::enableAll() -{ - enableAllGraspTypes(); - enableAllGraspAxes(); -} -void TwoFingerGraspCandidateConfig::disableAllGraspTypes() -{ - enable_corner_grasps_ = false; - enable_face_grasps_ = false; - enable_variable_angle_grasps_ = false; - enable_edge_grasps_ = false; -} -void TwoFingerGraspCandidateConfig::disableAllGraspAxes() -{ - generate_x_axis_grasps_ = false; - generate_y_axis_grasps_ = false; - generate_z_axis_grasps_ = false; -} -void TwoFingerGraspCandidateConfig::disableAll() -{ - disableAllGraspTypes(); - disableAllGraspAxes(); -} - -// Constructor -TwoFingerGraspGenerator::TwoFingerGraspGenerator(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, - bool verbose) - : GraspGenerator(visual_tools, verbose) -{ - auto two_finger_grasp_score_weights = std::make_shared<TwoFingerGraspScoreWeights>(); - grasp_score_weights_ = std::dynamic_pointer_cast<GraspScoreWeights>(two_finger_grasp_score_weights); - - grasp_candidate_config_ = TwoFingerGraspCandidateConfig(); -} - -void TwoFingerGraspGenerator::setGraspCandidateConfig(const TwoFingerGraspCandidateConfig& grasp_candidate_config) -{ - grasp_candidate_config_ = grasp_candidate_config; -} - -bool TwoFingerGraspGenerator::generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, - double height, const moveit_grasps::GraspDataPtr& grasp_data, - std::vector<GraspCandidatePtr>& grasp_candidates) -{ - auto two_finger_grasp_data = std::dynamic_pointer_cast<TwoFingerGraspData>(grasp_data); - if (!two_finger_grasp_data) - { - ROS_ERROR_STREAM_NAMED("grasp_generator", - "grasp_data is not castable to TwoFingerGraspData. Make sure you are using " - "the child class"); - return false; - } - return generateGrasps(cuboid_pose, depth, width, height, two_finger_grasp_data, grasp_candidates); -} - -bool TwoFingerGraspGenerator::generateGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, - double height, const TwoFingerGraspDataPtr& grasp_data, - std::vector<GraspCandidatePtr>& grasp_candidates) -{ - bool result = - generateFingerGrasps(cuboid_pose, depth, width, height, grasp_data, grasp_candidates, grasp_candidate_config_); - - if (result) - std::sort(grasp_candidates.begin(), grasp_candidates.end(), GraspFilter::compareGraspScores); - - return result; -} - -bool TwoFingerGraspGenerator::addGrasp(const Eigen::Isometry3d& grasp_pose_eef_mount, - const TwoFingerGraspDataPtr& grasp_data, const Eigen::Isometry3d& object_pose, - const Eigen::Vector3d& object_size, double object_width, - std::vector<GraspCandidatePtr>& grasp_candidates) -{ - // Transform the grasp pose eef mount to the tcp grasp pose - Eigen::Isometry3d grasp_pose_tcp = grasp_pose_eef_mount * grasp_data->tcp_to_eef_mount_.inverse(); - - // The new grasp - moveit_msgs::Grasp new_grasp; - - // Approach and retreat - aligned with eef to grasp transform - // set pregrasp - moveit_msgs::GripperTranslation pre_grasp_approach; - new_grasp.pre_grasp_approach.direction.header.stamp = ros::Time::now(); - new_grasp.pre_grasp_approach.desired_distance = grasp_data->grasp_max_depth_ + grasp_data->approach_distance_desired_; - new_grasp.pre_grasp_approach.min_distance = 0; // NOT IMPLEMENTED - new_grasp.pre_grasp_approach.direction.header.frame_id = grasp_data->parent_link_->getName(); - - Eigen::Vector3d grasp_approach_vector = -1 * grasp_data->tcp_to_eef_mount_.translation(); - grasp_approach_vector = grasp_approach_vector / grasp_approach_vector.norm(); - - new_grasp.pre_grasp_approach.direction.vector.x = grasp_approach_vector.x(); - new_grasp.pre_grasp_approach.direction.vector.y = grasp_approach_vector.y(); - new_grasp.pre_grasp_approach.direction.vector.z = grasp_approach_vector.z(); - - // set postgrasp - moveit_msgs::GripperTranslation post_grasp_retreat; - new_grasp.post_grasp_retreat.direction.header.stamp = ros::Time::now(); - new_grasp.post_grasp_retreat.desired_distance = grasp_data->grasp_max_depth_ + grasp_data->retreat_distance_desired_; - new_grasp.post_grasp_retreat.min_distance = 0; // NOT IMPLEMENTED - new_grasp.post_grasp_retreat.direction.header.frame_id = grasp_data->parent_link_->getName(); - new_grasp.post_grasp_retreat.direction.vector.x = -1 * grasp_approach_vector.x(); - new_grasp.post_grasp_retreat.direction.vector.y = -1 * grasp_approach_vector.y(); - new_grasp.post_grasp_retreat.direction.vector.z = -1 * grasp_approach_vector.z(); - - // set grasp pose - geometry_msgs::PoseStamped grasp_pose_msg; - grasp_pose_msg.header.stamp = ros::Time::now(); - grasp_pose_msg.header.frame_id = grasp_data->base_link_; - - // name the grasp - static std::size_t grasp_id = 0; - new_grasp.id = "Grasp" + boost::lexical_cast<std::string>(grasp_id); - grasp_id++; - - tf::poseEigenToMsg(grasp_pose_eef_mount, grasp_pose_msg.pose); - new_grasp.grasp_pose = grasp_pose_msg; - - // set grasp postures e.g. hand closed - new_grasp.grasp_posture = grasp_data->grasp_posture_; - - // set minimum opening of fingers for pre grasp approach - double min_finger_open_on_approach = object_width + 2 * grasp_data->grasp_padding_on_approach_; - double percent_open; - - // Create grasp with widest fingers possible ---------------------------------------------- - percent_open = 1.0; - if (!grasp_data->setGraspWidth(percent_open, min_finger_open_on_approach, new_grasp.pre_grasp_posture)) - { - debugFailedOpenGripper(percent_open, min_finger_open_on_approach, object_width, - grasp_data->grasp_padding_on_approach_); - return false; - } - - new_grasp.grasp_quality = scoreFingerGrasp(grasp_pose_tcp, grasp_data, object_pose, percent_open); - - // Show visualization for widest grasp - - grasp_candidates.push_back(std::make_shared<GraspCandidate>(new_grasp, grasp_data, object_pose)); - - // Create grasp with middle width fingers ------------------------------------------------- - percent_open = 0.5; - if (!grasp_data->setGraspWidth(percent_open, min_finger_open_on_approach, new_grasp.pre_grasp_posture)) - { - debugFailedOpenGripper(percent_open, min_finger_open_on_approach, object_width, - grasp_data->grasp_padding_on_approach_); - return false; - } - new_grasp.grasp_quality = scoreFingerGrasp(grasp_pose_tcp, grasp_data, object_pose, percent_open); - grasp_candidates.push_back(std::make_shared<GraspCandidate>(new_grasp, grasp_data, object_pose)); - - // Create grasp with fingers at minimum width --------------------------------------------- - percent_open = 0.0; - if (!grasp_data->setGraspWidth(percent_open, min_finger_open_on_approach, new_grasp.pre_grasp_posture)) - { - debugFailedOpenGripper(percent_open, min_finger_open_on_approach, object_width, - grasp_data->grasp_padding_on_approach_); - return false; - } - new_grasp.grasp_quality = scoreFingerGrasp(grasp_pose_tcp, grasp_data, object_pose, percent_open); - grasp_candidates.push_back(std::make_shared<GraspCandidate>(new_grasp, grasp_data, object_pose)); - - return true; -} - -bool TwoFingerGraspGenerator::generateCuboidAxisGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, - double height, grasp_axis_t axis, - const TwoFingerGraspDataPtr& grasp_data, - const TwoFingerGraspCandidateConfig& grasp_candidate_config, - std::vector<GraspCandidatePtr>& grasp_candidates) -{ - double finger_depth = grasp_data->grasp_max_depth_ - grasp_data->grasp_min_depth_; - double length_along_a, length_along_b, length_along_c; - double delta_a, delta_b, delta_f; - double alpha_x, alpha_y, alpha_z; - Eigen::Vector3d object_size(depth, width, height); - - double object_width; - EigenSTL::vector_Isometry3d grasp_poses_tcp; - - Eigen::Isometry3d grasp_pose_tcp = cuboid_pose; - Eigen::Vector3d a_dir, b_dir, c_dir; - - if (axis == X_AXIS) - { - length_along_a = width; - length_along_b = height; - length_along_c = depth; - a_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitY(); - b_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ(); - c_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitX(); - alpha_x = -M_PI / 2.0; - alpha_y = 0; - alpha_z = -M_PI / 2.0; - object_width = depth; - } - else if (axis == Y_AXIS) - { - length_along_a = depth; - length_along_b = height; - length_along_c = width; - a_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitX(); - b_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ(); - c_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitY(); - alpha_x = 0; - alpha_y = M_PI / 2.0; - alpha_z = M_PI; - object_width = width; - } - else // Z_AXIS - { - length_along_a = depth; - length_along_b = width; - length_along_c = height; - a_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitX(); - b_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitY(); - c_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ(); - alpha_x = M_PI / 2.0; - alpha_y = M_PI / 2.0; - alpha_z = 0; - object_width = height; - } - - double rotation_angles[3]; - rotation_angles[0] = alpha_x; - rotation_angles[1] = alpha_y; - rotation_angles[2] = alpha_z; - - a_dir = a_dir.normalized(); - b_dir = b_dir.normalized(); - c_dir = c_dir.normalized(); - - // Add grasps at corners, grasps are centroid aligned - double offset = 0.001; // back the palm off of the object slightly - Eigen::Vector3d corner_translation_a; - Eigen::Vector3d corner_translation_b; - double angle_res = grasp_data->angle_resolution_ * M_PI / 180.0; - std::size_t num_radial_grasps = ceil((M_PI / 2.0) / angle_res); - Eigen::Vector3d translation; - - if (grasp_candidate_config.enable_corner_grasps_) - { - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding corner grasps..."); - corner_translation_a = 0.5 * (length_along_a + offset) * a_dir; - corner_translation_b = 0.5 * (length_along_b + offset) * b_dir; - - if (num_radial_grasps <= 0) - num_radial_grasps = 1; - - // move to corner 0.5 * ( -a, -b) - translation = -corner_translation_a - corner_translation_b; - addCornerGraspsHelper(cuboid_pose, rotation_angles, translation, 0.0, num_radial_grasps, grasp_poses_tcp); - - // move to corner 0.5 * ( -a, +b) - translation = -corner_translation_a + corner_translation_b; - addCornerGraspsHelper(cuboid_pose, rotation_angles, translation, -M_PI / 2.0, num_radial_grasps, grasp_poses_tcp); - - // move to corner 0.5 * ( +a, +b) - translation = corner_translation_a + corner_translation_b; - addCornerGraspsHelper(cuboid_pose, rotation_angles, translation, M_PI, num_radial_grasps, grasp_poses_tcp); - - // move to corner 0.5 * ( +a, -b) - translation = corner_translation_a - corner_translation_b; - addCornerGraspsHelper(cuboid_pose, rotation_angles, translation, M_PI / 2.0, num_radial_grasps, grasp_poses_tcp); - } - std::size_t num_corner_grasps = grasp_poses_tcp.size(); - - // Create grasps along faces of cuboid, grasps are axis aligned - std::size_t num_grasps_along_a; - std::size_t num_grasps_along_b; - double rotation; - Eigen::Vector3d a_translation; - Eigen::Vector3d b_translation; - Eigen::Vector3d delta; - - // get exact deltas for sides from desired delta - num_grasps_along_a = floor((length_along_a - grasp_data->gripper_finger_width_) / grasp_data->grasp_resolution_) + 1; - num_grasps_along_b = floor((length_along_b - grasp_data->gripper_finger_width_) / grasp_data->grasp_resolution_) + 1; - - // if the gripper fingers are wider than the object we're trying to grasp, try with gripper aligned with - // top/center/bottom of object - // note that current implementation limits objects that are the same size as the gripper_finger_width to 1 grasp - if (num_grasps_along_a <= 0) - { - delta_a = length_along_a - grasp_data->gripper_finger_width_ / 2.0; - num_grasps_along_a = 3; - } - if (num_grasps_along_b <= 0) - { - delta_b = length_along_b - grasp_data->gripper_finger_width_ / 2.0; - num_grasps_along_b = 3; - } - - if (num_grasps_along_a == 1) - delta_a = 0; - else - delta_a = (length_along_a - grasp_data->gripper_finger_width_) / static_cast<double>(num_grasps_along_a - 1); - - if (num_grasps_along_b == 1) - delta_b = 0; - else - delta_b = (length_along_b - grasp_data->gripper_finger_width_) / static_cast<double>(num_grasps_along_b - 1); - - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "delta_a : delta_b = " << delta_a << " : " << delta_b); - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "num_grasps_along_a : num_grasps_along_b = " - << num_grasps_along_a << " : " << num_grasps_along_b); - - // TODO(mlautman): There is a bug with face grasps allowing the grasp generator to generate grasps where the gripper - // fingers - // are in collision with the object being grasped - if (grasp_candidate_config.enable_face_grasps_) - { - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding face grasps..."); - - a_translation = -(0.5 * (length_along_a + offset) * a_dir) - - 0.5 * (length_along_b - grasp_data->gripper_finger_width_) * b_dir - delta_b * b_dir; - b_translation = -0.5 * (length_along_a - grasp_data->gripper_finger_width_) * a_dir - delta_a * a_dir - - (0.5 * (length_along_b + offset) * b_dir); - - // grasps along -a_dir face - delta = delta_b * b_dir; - rotation = 0.0; - addFaceGraspsHelper(cuboid_pose, rotation_angles, a_translation, delta, rotation, num_grasps_along_b, - grasp_poses_tcp); - - // grasps along +b_dir face - rotation = -M_PI / 2.0; - delta = -delta_a * a_dir; - addFaceGraspsHelper(cuboid_pose, rotation_angles, -b_translation, delta, rotation, num_grasps_along_b, - grasp_poses_tcp); - - // grasps along +a_dir face - rotation = M_PI; - delta = -delta_b * b_dir; - addFaceGraspsHelper(cuboid_pose, rotation_angles, -a_translation, delta, rotation, num_grasps_along_b, - grasp_poses_tcp); - - // grasps along -b_dir face - rotation = M_PI / 2.0; - delta = delta_a * a_dir; - addFaceGraspsHelper(cuboid_pose, rotation_angles, b_translation, delta, rotation, num_grasps_along_b, - grasp_poses_tcp); - } - - // add grasps at variable angles - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding variable angle grasps..."); - Eigen::Isometry3d base_pose; - std::size_t num_grasps = grasp_poses_tcp.size(); - if (grasp_candidate_config.enable_variable_angle_grasps_) - { - // corner grasps at zero depth don't need variable angles - for (std::size_t i = num_corner_grasps; i < num_grasps; ++i) - { - base_pose = grasp_poses_tcp[i]; - - grasp_pose_tcp = base_pose * Eigen::AngleAxisd(angle_res, Eigen::Vector3d::UnitY()); - std::size_t max_iterations = M_PI / angle_res + 1; - std::size_t iterations = 0; - while (graspIntersectionHelper(cuboid_pose, depth, width, height, grasp_pose_tcp, grasp_data)) - { - grasp_poses_tcp.push_back(grasp_pose_tcp); - grasp_pose_tcp *= Eigen::AngleAxisd(angle_res, Eigen::Vector3d::UnitY()); - iterations++; - if (iterations > max_iterations) - { - ROS_WARN_STREAM_NAMED("cuboid_axis_grasps", "exceeded max iterations while creating variable angle grasps"); - break; - } - } - - iterations = 0; - grasp_pose_tcp = base_pose * Eigen::AngleAxisd(-angle_res, Eigen::Vector3d::UnitY()); - while (graspIntersectionHelper(cuboid_pose, depth, width, height, grasp_pose_tcp, grasp_data)) - { - grasp_poses_tcp.push_back(grasp_pose_tcp); - // visual_tools_->publishZArrow(grasp_pose_tcp, rviz_visual_tools::CYAN, rviz_visual_tools::XSMALL, 0.02); - grasp_pose_tcp *= Eigen::AngleAxisd(-angle_res, Eigen::Vector3d::UnitY()); - // ros::Duration(0.2).sleep(); - iterations++; - if (iterations > max_iterations) - { - ROS_WARN_STREAM_NAMED("cuboid_axis_grasps", "exceeded max iterations while creating variable angle grasps"); - break; - } - } - } - } - - if (grasp_candidate_config.enable_edge_grasps_) - { - // Add grasps along edges - // move grasp pose to edge of cuboid - double a_sign = 1.0; - double b_sign = 1.0; - double a_rot_sign = 1.0; - double b_rot_sign = 1.0; - - if (axis == Y_AXIS) - { - a_sign = -1.0; - b_rot_sign = -1.0; - } - - if (axis == Z_AXIS) - { - a_sign = -1.0; - b_sign = -1.0; - a_rot_sign = -1.0; - b_rot_sign = -1.0; - } - - a_translation = -0.5 * (length_along_a + offset) * a_dir - - 0.5 * (length_along_b - grasp_data->gripper_finger_width_) * b_dir - delta_b * b_dir - - 0.5 * (length_along_c + offset) * c_dir * a_sign; - b_translation = -0.5 * (length_along_a - grasp_data->gripper_finger_width_) * a_dir - delta_a * a_dir - - (0.5 * (length_along_b + offset) * b_dir) - 0.5 * (length_along_c + offset) * c_dir * b_sign; - - // grasps along -a_dir face - delta = delta_b * b_dir; - rotation = 0.0; - addEdgeGraspsHelper(cuboid_pose, rotation_angles, a_translation, delta, rotation, num_grasps_along_b, - grasp_poses_tcp, -M_PI / 4.0 * a_rot_sign); - - // grasps along +b_dir face - rotation = -M_PI / 2.0; - delta = -delta_a * a_dir; - addEdgeGraspsHelper(cuboid_pose, rotation_angles, -b_translation, delta, rotation, num_grasps_along_b, - grasp_poses_tcp, M_PI / 4.0 * b_rot_sign); - - // grasps along +a_dir face - rotation = M_PI; - delta = -delta_b * b_dir; - addEdgeGraspsHelper(cuboid_pose, rotation_angles, -a_translation, delta, rotation, num_grasps_along_b, - grasp_poses_tcp, M_PI / 4.0 * a_rot_sign); - - // grasps along -b_dir face - rotation = M_PI / 2.0; - delta = delta_a * a_dir; - addEdgeGraspsHelper(cuboid_pose, rotation_angles, b_translation, delta, rotation, num_grasps_along_b, - grasp_poses_tcp, -M_PI / 4.0 * b_rot_sign); - } - // Add grasps at variable depths - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding depth grasps..."); - std::size_t num_depth_grasps = ceil(finger_depth / grasp_data->grasp_depth_resolution_); - if (num_depth_grasps <= 0) - num_depth_grasps = 1; - delta_f = finger_depth / static_cast<double>(num_depth_grasps); - - num_grasps = grasp_poses_tcp.size(); - Eigen::Vector3d grasp_dir; - Eigen::Isometry3d depth_pose; - - for (std::size_t i = 0; i < num_grasps; ++i) - { - grasp_dir = grasp_poses_tcp[i].rotation() * Eigen::Vector3d::UnitZ(); - depth_pose = grasp_poses_tcp[i]; - for (std::size_t j = 0; j < num_depth_grasps; ++j) - { - depth_pose.translation() += delta_f * grasp_dir; - grasp_poses_tcp.push_back(depth_pose); - } - } - - // add grasps in both directions - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "adding bi-directional grasps..."); - num_grasps = grasp_poses_tcp.size(); - for (std::size_t i = 0; i < num_grasps; ++i) - { - grasp_pose_tcp = grasp_poses_tcp[i]; - grasp_pose_tcp *= Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()); - grasp_poses_tcp.push_back(grasp_pose_tcp); - } - - // compute min/max distances to object - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps", "computing min/max grasp distance..."); - num_grasps = grasp_poses_tcp.size(); - min_grasp_distance_ = std::numeric_limits<double>::max(); - max_grasp_distance_ = std::numeric_limits<double>::min(); - min_translations_ = Eigen::Vector3d(std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), - std::numeric_limits<double>::max()); - max_translations_ = Eigen::Vector3d(std::numeric_limits<double>::min(), std::numeric_limits<double>::min(), - std::numeric_limits<double>::min()); - double grasp_distance; - - for (std::size_t i = 0; i < num_grasps; ++i) - { - grasp_pose_tcp = grasp_poses_tcp[i]; - grasp_distance = (grasp_pose_tcp.translation() - cuboid_pose.translation()).norm(); - if (grasp_distance > max_grasp_distance_) - max_grasp_distance_ = grasp_distance; - - if (grasp_distance < min_grasp_distance_) - min_grasp_distance_ = grasp_distance; - - for (std::size_t j = 0; j < 3; ++j) - { - if (grasp_pose_tcp.translation()[j] < min_translations_[j]) - min_translations_[j] = grasp_pose_tcp.translation()[j]; - - if (grasp_pose_tcp.translation()[j] > max_translations_[j]) - max_translations_[j] = grasp_pose_tcp.translation()[j]; - } - } - - ROS_DEBUG_STREAM_NAMED("grasp_generator.add", - "min/max distance = " << min_grasp_distance_ << ", " << max_grasp_distance_); - - // add all poses as possible grasps - std::size_t num_grasps_added = 0; - - for (std::size_t i = 0; i < grasp_poses_tcp.size(); ++i) - { - Eigen::Isometry3d grasp_pose_eef_mount = grasp_poses_tcp[i] * grasp_data->tcp_to_eef_mount_; - if (!addGrasp(grasp_pose_eef_mount, grasp_data, cuboid_pose, object_size, object_width, grasp_candidates)) - { - ROS_DEBUG_STREAM_NAMED("grasp_generator.add", "Unable to add grasp - function returned false"); - } - else - num_grasps_added++; - } - ROS_INFO_STREAM_NAMED("grasp_generator.add", "\033[1;36madded " << num_grasps_added << " of " - << grasp_poses_tcp.size() - << " grasp poses created\033[0m"); - return true; -} - -std::size_t TwoFingerGraspGenerator::addCornerGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3], - const Eigen::Vector3d& translation, double corner_rotation, - std::size_t num_radial_grasps, - EigenSTL::vector_Isometry3d& grasp_poses_tcp) -{ - std::size_t num_grasps_added = 0; - double delta_angle = (M_PI / 2.0) / static_cast<double>(num_radial_grasps + 1); - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "delta_angle = " << delta_angle); - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_radial_grasps = " << num_radial_grasps); - - // rotate & translate pose to be aligned with edge of cuboid - Eigen::Isometry3d grasp_pose_tcp = pose; - grasp_pose_tcp *= Eigen::AngleAxisd(rotation_angles[0], Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(rotation_angles[1], Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rotation_angles[2], Eigen::Vector3d::UnitZ()); - grasp_pose_tcp *= Eigen::AngleAxisd(corner_rotation, Eigen::Vector3d::UnitY()); - grasp_pose_tcp.translation() += translation; - - for (std::size_t i = 0; i < num_radial_grasps; ++i) - { - // Eigen::Vector3d grasp_dir = grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ(); - // Eigen::Isometry3d radial_pose = grasp_pose_tcp; - grasp_pose_tcp *= Eigen::AngleAxisd(delta_angle, Eigen::Vector3d::UnitY()); - grasp_poses_tcp.push_back(grasp_pose_tcp); - num_grasps_added++; - } - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps_added : grasp_poses_tcp.size() = " - << num_grasps_added << " : " << grasp_poses_tcp.size()); - return num_grasps_added; -} - -std::size_t TwoFingerGraspGenerator::addFaceGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3], - const Eigen::Vector3d& translation, - const Eigen::Vector3d& delta, double alignment_rotation, - std::size_t num_grasps, - EigenSTL::vector_Isometry3d& grasp_poses_tcp) -{ - std::size_t num_grasps_added = 0; - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "delta = \n" << delta); - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps = " << num_grasps); - - Eigen::Isometry3d grasp_pose_tcp = pose; - grasp_pose_tcp *= Eigen::AngleAxisd(rotation_angles[0], Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(rotation_angles[1], Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rotation_angles[2], Eigen::Vector3d::UnitZ()); - grasp_pose_tcp *= Eigen::AngleAxisd(alignment_rotation, Eigen::Vector3d::UnitY()); - grasp_pose_tcp.translation() += translation; - - for (std::size_t i = 0; i < num_grasps; ++i) - { - grasp_pose_tcp.translation() += delta; - grasp_poses_tcp.push_back(grasp_pose_tcp); - num_grasps_added++; - } - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps_added : grasp_poses_tcp.size() = " - << num_grasps_added << " : " << grasp_poses_tcp.size()); - return true; -} - -std::size_t TwoFingerGraspGenerator::addEdgeGraspsHelper(const Eigen::Isometry3d& pose, double rotation_angles[3], - const Eigen::Vector3d& translation, - const Eigen::Vector3d& delta, double alignment_rotation, - std::size_t num_grasps, - EigenSTL::vector_Isometry3d& grasp_poses_tcp, - double corner_rotation) -{ - std::size_t num_grasps_added = 0; - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "delta = \n" << delta); - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps = " << num_grasps); - - Eigen::Isometry3d grasp_pose_tcp = pose; - grasp_pose_tcp *= Eigen::AngleAxisd(rotation_angles[0], Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(rotation_angles[1], Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rotation_angles[2], Eigen::Vector3d::UnitZ()); - grasp_pose_tcp *= Eigen::AngleAxisd(alignment_rotation, Eigen::Vector3d::UnitY()); - - // rotate towards cuboid - grasp_pose_tcp *= Eigen::AngleAxisd(corner_rotation, Eigen::Vector3d::UnitX()); - grasp_pose_tcp.translation() += translation; - - for (std::size_t i = 0; i < num_grasps; ++i) - { - grasp_pose_tcp.translation() += delta; - grasp_poses_tcp.push_back(grasp_pose_tcp); - num_grasps_added++; - } - ROS_DEBUG_STREAM_NAMED("cuboid_axis_grasps.helper", "num_grasps_added : grasp_poses_tcp.size() = " - << num_grasps_added << " : " << grasp_poses_tcp.size()); - return true; -} - -bool TwoFingerGraspGenerator::graspIntersectionHelper(const Eigen::Isometry3d& cuboid_pose, double depth, double width, - double height, const Eigen::Isometry3d& grasp_pose_tcp, - const TwoFingerGraspDataPtr& grasp_data) -{ - // TODO(davetcoleman): add parameter to enable vizualization commented lines after further testing - - // get line segment from grasp point to fingertip - Eigen::Vector3d point_a = grasp_pose_tcp.translation(); - Eigen::Vector3d point_b = - point_a + grasp_pose_tcp.rotation() * Eigen::Vector3d::UnitZ() * grasp_data->grasp_max_depth_; - - // translate points into cuboid coordinate system - point_a = cuboid_pose.inverse() * point_a; // T_cuboid-world * p_world = p_cuboid - point_b = cuboid_pose.inverse() * point_b; - - double t, u, v; - Eigen::Vector3d intersection; - // check if line segment intersects XY faces of cuboid (z = +/- height/2) - t = (height / 2.0 - point_a[2]) / (point_b[2] - point_a[2]); // parameterization of line segment in 3d - if (intersectionHelper(t, point_a[0], point_a[1], point_b[0], point_b[1], depth, width, u, v)) - { - return true; - } - - t = (-height / 2.0 - point_a[2]) / (point_b[2] - point_a[2]); - if (intersectionHelper(t, point_a[0], point_a[1], point_b[0], point_b[1], depth, width, u, v)) - { - return true; - } - - // check if line segment intersects XZ faces of cuboid (y = +/- width/2) - t = (width / 2.0 - point_a[1]) / (point_b[1] - point_a[1]); - if (intersectionHelper(t, point_a[0], point_a[2], point_b[0], point_b[2], depth, height, u, v)) - { - return true; - } - - t = (-width / 2.0 - point_a[1]) / (point_b[1] - point_a[1]); - if (intersectionHelper(t, point_a[0], point_a[2], point_b[0], point_b[2], depth, height, u, v)) - { - return true; - } - - // check if line segment intersects YZ faces of cuboid (x = +/- depth/2) - t = (depth / 2.0 - point_a[0]) / (point_b[0] - point_a[0]); - if (intersectionHelper(t, point_a[1], point_a[2], point_b[1], point_b[2], width, height, u, v)) - { - return true; - } - - t = (-depth / 2.0 - point_a[0]) / (point_b[0] - point_a[0]); - if (intersectionHelper(t, point_a[1], point_a[2], point_b[1], point_b[2], width, height, u, v)) - { - return true; - } - - // no intersection found - return false; -} - -bool TwoFingerGraspGenerator::intersectionHelper(double t, double u1, double v1, double u2, double v2, double a, - double b, double& u, double& v) -{ - // plane must cross through our line segment - if (t >= 0 && t <= 1) - { - u = u1 + t * (u2 - u1); - v = v1 + t * (v2 - v1); - - if (u >= -a / 2 && u <= a / 2 && v >= -b / 2 && v <= b / 2) - return true; - } - - return false; -} - -double TwoFingerGraspGenerator::scoreFingerGrasp(const Eigen::Isometry3d& grasp_pose_tcp, - const TwoFingerGraspDataPtr& grasp_data, - const Eigen::Isometry3d& object_pose, double percent_open) -{ - static const std::string logger_name = "grasp_generator.scoreGrasp"; - ROS_DEBUG_STREAM_NAMED(logger_name, "starting to score grasp..."); - - // get portion of score based on the gripper's opening width on approach - double width_score = TwoFingerGraspScorer::scoreGraspWidth(grasp_data, percent_open); - - // get portion of score based on the pinchers being down - Eigen::Vector3d orientation_scores = - TwoFingerGraspScorer::scoreRotationsFromDesired(grasp_pose_tcp, ideal_grasp_pose_); - - // get portion of score based on the distance of the grasp pose to the object pose - - // NOTE: when this function is called we've lost the references to the acutal size of the object. - // max_distance should be the length of the fingers minus some minimum amount that the fingers need to grip an object - // since we don't know the distance from the centoid of the object to the edge of the object, this is set as an - // arbitrary number given our target object set - double distance_score = TwoFingerGraspScorer::scoreDistanceToPalm(grasp_pose_tcp, grasp_data, object_pose, - min_grasp_distance_, max_grasp_distance_); - - // should really change this to be like orienation_scores so we can score any translation - Eigen::Vector3d translation_scores = - TwoFingerGraspScorer::scoreGraspTranslation(grasp_pose_tcp, min_translations_, max_translations_); - - // want minimum translation - translation_scores *= -1.0; - translation_scores += Eigen::Vector3d(1.0, 1.0, 1.0); - - double total_score = 0; - auto two_finger_grasp_score_weights = std::dynamic_pointer_cast<TwoFingerGraspScoreWeights>(grasp_score_weights_); - if (two_finger_grasp_score_weights) - { - total_score = two_finger_grasp_score_weights->computeScore(orientation_scores, translation_scores, distance_score, - width_score, getVerbose()); - } - else - { - ROS_WARN_NAMED(logger_name, "Failed to cast grasp_score_weights_ as TwoFingerGraspScoreWeights. continuing without " - "finger specific scores"); - total_score = grasp_score_weights_->computeScore(orientation_scores, translation_scores, getVerbose()); - } - - return total_score; -} - -bool TwoFingerGraspGenerator::generateFingerGrasps(const Eigen::Isometry3d& cuboid_pose, double depth, double width, - double height, const TwoFingerGraspDataPtr& grasp_data, - std::vector<GraspCandidatePtr>& grasp_candidates, - const TwoFingerGraspCandidateConfig& grasp_candidate_config) -{ - // Generate grasps over axes that aren't too wide to grip - // Most default type of grasp is X axis - TwoFingerGraspCandidateConfig grasp_candidate_config_copy(grasp_candidate_config); - if (grasp_candidate_config_copy.generate_x_axis_grasps_) - { - ROS_DEBUG_STREAM_NAMED("grasp_generator", "Generating grasps around x-axis of cuboid"); - if (depth > grasp_data->max_grasp_width_) // depth = size along x-axis - { - grasp_candidate_config_copy.disableAllGraspTypes(); - grasp_candidate_config_copy.enable_edge_grasps_ = grasp_candidate_config.enable_edge_grasps_; - grasp_candidate_config_copy.enable_corner_grasps_ = grasp_candidate_config.enable_corner_grasps_; - } - generateCuboidAxisGrasps(cuboid_pose, depth, width, height, X_AXIS, grasp_data, grasp_candidate_config_copy, - grasp_candidates); - } - - grasp_candidate_config_copy = grasp_candidate_config; - if (grasp_candidate_config_copy.generate_y_axis_grasps_) - { - ROS_DEBUG_STREAM_NAMED("grasp_generator", "Generating grasps around y-axis of cuboid"); - if (width > grasp_data->max_grasp_width_) // width = size along y-axis - { - grasp_candidate_config_copy.disableAllGraspTypes(); - grasp_candidate_config_copy.enable_edge_grasps_ = grasp_candidate_config.enable_edge_grasps_; - grasp_candidate_config_copy.enable_corner_grasps_ = grasp_candidate_config.enable_corner_grasps_; - } - generateCuboidAxisGrasps(cuboid_pose, depth, width, height, Y_AXIS, grasp_data, grasp_candidate_config_copy, - grasp_candidates); - } - - grasp_candidate_config_copy = grasp_candidate_config; - if (grasp_candidate_config_copy.generate_z_axis_grasps_) - { - ROS_DEBUG_STREAM_NAMED("grasp_generator", "Generating grasps around z-axis of cuboid"); - if (height > grasp_data->max_grasp_width_) // height = size along z-axis - { - grasp_candidate_config_copy.disableAllGraspTypes(); - grasp_candidate_config_copy.enable_edge_grasps_ = grasp_candidate_config.enable_edge_grasps_; - grasp_candidate_config_copy.enable_corner_grasps_ = grasp_candidate_config.enable_corner_grasps_; - } - generateCuboidAxisGrasps(cuboid_pose, depth, width, height, Z_AXIS, grasp_data, grasp_candidate_config_copy, - grasp_candidates); - } - - if (!grasp_candidates.size()) - ROS_WARN_STREAM_NAMED("grasp_generator", "Generated 0 grasps"); - else - ROS_INFO_STREAM_NAMED("grasp_generator", "Generated " << grasp_candidates.size() << " grasps"); - - // Visualize animated grasps that have been generated - if (show_prefiltered_grasps_) - { - ROS_DEBUG_STREAM_NAMED("grasp_generator", "Animating all generated (candidate) grasps before filtering"); - visualizeAnimatedGrasps(grasp_candidates, grasp_data->ee_jmg_, show_prefiltered_grasps_speed_); - } - - return true; -} - -} // namespace moveit_grasps diff --git a/src/mtc/src/two_finger_grasp_scorer.cpp b/src/mtc/src/two_finger_grasp_scorer.cpp deleted file mode 100644 index c542176fc239759108533e31b4a939b6e3a5269e..0000000000000000000000000000000000000000 --- a/src/mtc/src/two_finger_grasp_scorer.cpp +++ /dev/null @@ -1,102 +0,0 @@ -/********************************************************************* - * Software License Agreement ("Modified BSD License") - * - * Copyright (c) 2014, University of Colorado, Boulder - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Univ of CO, Boulder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/** - * Authors : Andy McEvoy ( mcevoy.andy@gmail.com ) - * Desc : Functions for scoring grasps. See *.h file for documentation - */ - -#include "../include/two_finger_grasp_scorer.h" - -namespace moveit_grasps -{ -double TwoFingerGraspScoreWeights::computeScore(const Eigen::Vector3d& orientation_scores, - const Eigen::Vector3d& translation_scores, double depth_score, - double width_score, bool verbose) const -{ - double total_score = GraspScoreWeights::computeScore(orientation_scores, translation_scores, false) * - GraspScoreWeights::getWeightTotal(); - total_score += depth_score * depth_score_weight_ + width_score * width_score_weight_; - - total_score /= getWeightTotal(); - - if (verbose) - { - static const std::string logger_name = "grasp_scorer.compute_score"; - // clang-format off - ROS_DEBUG_STREAM_NAMED(logger_name, "Two Finger Grasp score: "); - ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.x = " << orientation_scores[0] << "\tweight = "<< orientation_x_score_weight_); - ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.y = " << orientation_scores[1] << "\tweight = "<< orientation_y_score_weight_); - ROS_DEBUG_STREAM_NAMED(logger_name, "\torientation_score.z = " << orientation_scores[2] << "\tweight = "<< orientation_z_score_weight_); - ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.x = " << translation_scores[0] << "\tweight = "<< translation_x_score_weight_); - ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.y = " << translation_scores[1] << "\tweight = "<< translation_y_score_weight_); - ROS_DEBUG_STREAM_NAMED(logger_name, "\ttranslation_score.z = " << translation_scores[2] << "\tweight = "<< translation_z_score_weight_); - ROS_DEBUG_STREAM_NAMED(logger_name, "\tdepth_score = " << depth_score << "\tweight = "<< depth_score_weight_); - ROS_DEBUG_STREAM_NAMED(logger_name, "\twidth_score = " << width_score << "\tweight = "<< width_score_weight_); - // Total - ROS_DEBUG_STREAM_NAMED(logger_name, "\ttotal_score = " << total_score); - // clang-format on - } - return total_score; -} - -double TwoFingerGraspScoreWeights::getWeightTotal() const -{ - return GraspScoreWeights::getWeightTotal() + depth_score_weight_ + width_score_weight_; -} - -double TwoFingerGraspScorer::scoreGraspWidth(const TwoFingerGraspDataPtr& grasp_data, double percent_open) -{ - ROS_DEBUG_STREAM_NAMED("grasp_scorer.graspWidth", "raw score = " << percent_open); - return pow(percent_open, 2); -} - -double TwoFingerGraspScorer::scoreDistanceToPalm(const Eigen::Isometry3d& grasp_pose_tcp, - const TwoFingerGraspDataPtr& grasp_data, - const Eigen::Isometry3d& object_pose, double min_grasp_distance, - double max_grasp_distance) -{ - double distance = (grasp_pose_tcp.translation() - object_pose.translation()).norm(); - ROS_DEBUG_STREAM_NAMED("grasp_scorer.distance", - "distance = " << distance << ", " << min_grasp_distance << ":" << max_grasp_distance); - - double score = 1.0 - (distance - min_grasp_distance) / (max_grasp_distance - min_grasp_distance); - - ROS_DEBUG_STREAM_NAMED("grasp_scorer.distance", "raw score = " << score); - if (score < 0) - ROS_WARN_STREAM_NAMED("grasp_scorer.distance", "score < 0!"); - return pow(score, 4); -} - -} // namespace moveit_grasps