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}/introspection.h
|
||||
${PROJECT_INCLUDE}/marker_tools.h
|
||||
${PROJECT_INCLUDE}/merge.h
|
||||
${PROJECT_INCLUDE}/properties.h
|
||||
${PROJECT_INCLUDE}/stage.h
|
||||
${PROJECT_INCLUDE}/stage_p.h
|
||||
@ -18,6 +19,7 @@ add_library(${PROJECT_NAME}
|
||||
container.cpp
|
||||
introspection.cpp
|
||||
marker_tools.cpp
|
||||
merge.cpp
|
||||
properties.cpp
|
||||
stage.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