mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
utility functions to merge multiple RobotTrajectories
This commit is contained in:
parent
2c08d9080d
commit
03092e7e45
59
core/include/moveit/task_constructor/merge.h
Normal file
59
core/include/moveit/task_constructor/merge.h
Normal file
@ -0,0 +1,59 @@
|
|||||||
|
/*********************************************************************
|
||||||
|
* Software License Agreement (BSD License)
|
||||||
|
*
|
||||||
|
* Copyright (c) 2017, Bielefeld University
|
||||||
|
* 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 Bielefeld University 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: Luca Lach, Robert Haschke */
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <moveit/task_constructor/storage.h>
|
||||||
|
#include <moveit/robot_model/robot_model.h>
|
||||||
|
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||||
|
|
||||||
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
|
|
||||||
|
/// create a new JointModelGroup comprising all joints of the given groups
|
||||||
|
moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::JointModelGroup*>& groups);
|
||||||
|
|
||||||
|
/** merge all sub trajectories into a single RobotTrajectory for parallel execution
|
||||||
|
*
|
||||||
|
* As the RobotTrajectory maintains a pointer to the underlying JointModelGroup
|
||||||
|
* (to know about the involved joint names), a merged JointModelGroup needs to be passed
|
||||||
|
* or created on the fly. This JMG needs to stay alive during the lifetime of the trajectory.
|
||||||
|
* For now, only the trajectory path is considered. Timings, velocities, etc. are ignored.
|
||||||
|
*/
|
||||||
|
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
|
||||||
|
core::JointModelGroup*& merged_group);
|
||||||
|
|
||||||
|
} }
|
||||||
@ -4,6 +4,7 @@ add_library(${PROJECT_NAME}
|
|||||||
${PROJECT_INCLUDE}/cost_queue.h
|
${PROJECT_INCLUDE}/cost_queue.h
|
||||||
${PROJECT_INCLUDE}/introspection.h
|
${PROJECT_INCLUDE}/introspection.h
|
||||||
${PROJECT_INCLUDE}/marker_tools.h
|
${PROJECT_INCLUDE}/marker_tools.h
|
||||||
|
${PROJECT_INCLUDE}/merge.h
|
||||||
${PROJECT_INCLUDE}/properties.h
|
${PROJECT_INCLUDE}/properties.h
|
||||||
${PROJECT_INCLUDE}/stage.h
|
${PROJECT_INCLUDE}/stage.h
|
||||||
${PROJECT_INCLUDE}/stage_p.h
|
${PROJECT_INCLUDE}/stage_p.h
|
||||||
@ -18,6 +19,7 @@ add_library(${PROJECT_NAME}
|
|||||||
container.cpp
|
container.cpp
|
||||||
introspection.cpp
|
introspection.cpp
|
||||||
marker_tools.cpp
|
marker_tools.cpp
|
||||||
|
merge.cpp
|
||||||
properties.cpp
|
properties.cpp
|
||||||
stage.cpp
|
stage.cpp
|
||||||
storage.cpp
|
storage.cpp
|
||||||
|
|||||||
133
core/src/merge.cpp
Normal file
133
core/src/merge.cpp
Normal file
@ -0,0 +1,133 @@
|
|||||||
|
/*********************************************************************
|
||||||
|
* Software License Agreement (BSD License)
|
||||||
|
*
|
||||||
|
* Copyright (c) 2017, Hamburg University
|
||||||
|
* Copyright (c) 2017, Bielefeld University
|
||||||
|
* 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 Bielefeld University 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: Luca Lach, Robert Haschke */
|
||||||
|
|
||||||
|
#include <moveit/task_constructor/merge.h>
|
||||||
|
|
||||||
|
namespace moveit { namespace task_constructor {
|
||||||
|
|
||||||
|
moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::JointModelGroup*>& groups)
|
||||||
|
{
|
||||||
|
if (groups.size() <= 1)
|
||||||
|
throw std::runtime_error("Expected multiple groups");
|
||||||
|
|
||||||
|
const moveit::core::RobotModel* const robot_model = &groups[0]->getParentModel();
|
||||||
|
|
||||||
|
std::set<const moveit::core::JointModel*> jset;
|
||||||
|
std::vector<std::string> names;
|
||||||
|
for (const moveit::core::JointModelGroup* jmg : groups) {
|
||||||
|
// sanity check: all groups must share the same robot model
|
||||||
|
if (&jmg->getParentModel() != robot_model)
|
||||||
|
throw std::runtime_error("groups refer to different robot models");
|
||||||
|
|
||||||
|
const auto& joints = jmg->getJointModels();
|
||||||
|
jset.insert(joints.cbegin(), joints.cend());
|
||||||
|
names.push_back(jmg->getName());
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<const moveit::core::JointModel*> joints(jset.cbegin(), jset.cend());
|
||||||
|
// attention: do not use getConfig()
|
||||||
|
return new moveit::core::JointModelGroup("", srdf::Model::Group(), joints, robot_model);
|
||||||
|
}
|
||||||
|
|
||||||
|
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
|
||||||
|
moveit::core::JointModelGroup*& merged_group)
|
||||||
|
{
|
||||||
|
if (sub_trajectories.size() <= 1)
|
||||||
|
throw std::runtime_error("Expected multiple sub solutions");
|
||||||
|
|
||||||
|
const std::vector<const moveit::core::JointModel*> *merged_joints
|
||||||
|
= merged_group ? &merged_group->getJointModels() : nullptr;
|
||||||
|
std::set<const moveit::core::JointModel*> jset;
|
||||||
|
|
||||||
|
// sanity checks: all sub solutions must share the same robot model and use disjoint joint sets
|
||||||
|
const moveit::core::RobotModelConstPtr& robot_model = sub_trajectories[0]->getRobotModel();
|
||||||
|
size_t max_num_joints = 0; // maximum number of joints in sub groups
|
||||||
|
for (const robot_trajectory::RobotTrajectoryPtr& sub : sub_trajectories) {
|
||||||
|
if (sub->getRobotModel() != robot_model)
|
||||||
|
throw std::runtime_error("subsolutions refer to multiple robot models");
|
||||||
|
|
||||||
|
const moveit::core::JointModelGroup* jmg = sub->getGroup();
|
||||||
|
const auto& joints = jmg->getJointModels();
|
||||||
|
if (merged_joints) { // validate that the joint model is known in merged_group
|
||||||
|
for (const moveit::core::JointModel* jm : joints) {
|
||||||
|
if (std::find(merged_joints->cbegin(), merged_joints->cend(), jm) == merged_joints->cend())
|
||||||
|
throw std::runtime_error("subsolutions refers to unknown joint: " + jm->getName());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
max_num_joints = std::max(max_num_joints, joints.size());
|
||||||
|
size_t expected_size = jset.size() + joints.size();
|
||||||
|
jset.insert(joints.cbegin(), joints.cend());
|
||||||
|
if (jset.size() != expected_size)
|
||||||
|
throw std::runtime_error("joint sets of subsolutions are not disjoint");
|
||||||
|
}
|
||||||
|
|
||||||
|
// create merged_group if necessary
|
||||||
|
if (!merged_group) {
|
||||||
|
std::vector<const moveit::core::JointModel*> joints(jset.cbegin(), jset.cend());
|
||||||
|
merged_group = new moveit::core::JointModelGroup("", srdf::Model::Group(), joints, robot_model.get());
|
||||||
|
}
|
||||||
|
|
||||||
|
// do the actual trajectory merging
|
||||||
|
auto merged_traj = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model, merged_group);
|
||||||
|
std::vector<double> values;
|
||||||
|
values.reserve(max_num_joints);
|
||||||
|
|
||||||
|
while (true) {
|
||||||
|
bool finished = true;
|
||||||
|
size_t index = merged_traj->getWayPointCount();
|
||||||
|
auto merged_state = index == 0 ? std::make_shared<robot_state::RobotState>(robot_model)
|
||||||
|
: std::make_shared<robot_state::RobotState>(merged_traj->getLastWayPoint());
|
||||||
|
for (const robot_trajectory::RobotTrajectoryPtr& sub : sub_trajectories) {
|
||||||
|
if (index >= sub->getWayPointCount())
|
||||||
|
continue; // no more waypoints in this sub solution
|
||||||
|
|
||||||
|
finished = false; // there was a waypoint, continue while loop
|
||||||
|
const robot_state::RobotState& sub_state = sub->getWayPoint(index);
|
||||||
|
sub_state.copyJointGroupPositions(sub->getGroup(), values);
|
||||||
|
merged_state->setJointGroupPositions(sub->getGroup(), values);
|
||||||
|
}
|
||||||
|
if (finished)
|
||||||
|
break;
|
||||||
|
|
||||||
|
// add waypoint without timing
|
||||||
|
merged_state->update();
|
||||||
|
merged_traj->addSuffixWayPoint(merged_state, 0.0);
|
||||||
|
}
|
||||||
|
return merged_traj;
|
||||||
|
}
|
||||||
|
|
||||||
|
} }
|
||||||
Loading…
Reference in New Issue
Block a user