Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions robowflex_dart/include/robowflex_dart/robot.h
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,10 @@
#ifndef ROBOWFLEX_DART_ROBOT_
#define ROBOWFLEX_DART_ROBOT_

#ifndef ROBOWFLEX_DART_ONLY
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_state/robot_state.h>
#endif

#include <dart/dynamics/Skeleton.hpp>
#include <dart/collision/CollisionFilter.hpp>
Expand Down Expand Up @@ -95,6 +97,7 @@ namespace robowflex
/** \name State Operations
\{ */

#ifndef ROBOWFLEX_DART_ONLY
/** \brief Set the current state of this robot from a MoveIt message.
* \param[in] msg Message to set state to.
*/
Expand Down Expand Up @@ -139,6 +142,7 @@ namespace robowflex
*/
void setMoveItJMGFromState(const std::string &jmg, Eigen::Ref<Eigen::VectorXd> vec) const;

#endif
/** \} */

/** \name Group Operations
Expand Down
2 changes: 2 additions & 0 deletions robowflex_dart/src/robot.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -461,6 +461,7 @@ void Robot::setNamedGroupState(const std::string &group, const std::string &name
it->second = q;
}

#ifndef ROBOWFLEX_DART_ONLY
void Robot::setStateFromMoveItMsg(const moveit_msgs::RobotState &msg)
{
for (std::size_t i = 0; i < msg.joint_state.name.size(); ++i)
Expand Down Expand Up @@ -564,6 +565,7 @@ void Robot::setMoveItJMGFromState(const std::string &jmg, Eigen::Ref<Eigen::Vect
setMoveItStateFromState(*robot_->getScratchState()); // copy current state
robot_->getScratchState()->copyJointGroupPositions(jmg, vec.data()); // copy JMG state
}
#endif

void Robot::processGroup(const std::string &group)
{
Expand Down