diff --git a/free_gait_action_loader/src/free_gait_action_loader/action_handling.py b/free_gait_action_loader/src/free_gait_action_loader/action_handling.py index 331dfeb..12ffd6d 100644 --- a/free_gait_action_loader/src/free_gait_action_loader/action_handling.py +++ b/free_gait_action_loader/src/free_gait_action_loader/action_handling.py @@ -69,6 +69,7 @@ def _initialize(self, package_path, parameters): self.name = parameters['name'] if 'file' in parameters: self.file = abspath(join(package_path, parameters['file'])) + rospy.loginfo(self.file) if 'type' in parameters: self.type = ActionType.from_text(parameters['type']) if 'description' in parameters: @@ -95,11 +96,13 @@ def update(self): self.actions = [] rospack = rospkg.RosPack() packages = rospack.get_depends_on(self.name, implicit=False) - rospy.loginfo(packages) + rospy.loginfo(rospack) for package in packages: rospy.loginfo(package) manifest = rospack.get_manifest(package) + rospy.loginfo(manifest.exports) file_path = manifest.get_export(self.name, 'actions') + rospy.loginfo(file_path) if not file_path: continue elif len(file_path) != 1: @@ -114,7 +117,9 @@ def update(self): package_path = rospack.get_path(package) parameters = load_file(file_path) + # rospy.loginfo(parameters[0][0]['actions']) for action_parameters in parameters[0][0]['actions']: + rospy.loginfo(action_parameters) entry = ActionEntry(package_path, action_parameters['action']) self.actions.append(entry) diff --git a/free_gait_action_loader/src/free_gait_action_loader/action_handling.pyc b/free_gait_action_loader/src/free_gait_action_loader/action_handling.pyc index f5a41c4..3f42cff 100644 Binary files a/free_gait_action_loader/src/free_gait_action_loader/action_handling.pyc and b/free_gait_action_loader/src/free_gait_action_loader/action_handling.pyc differ diff --git a/free_gait_core/CMakeLists.txt b/free_gait_core/CMakeLists.txt index 44fe48b..4948cac 100644 --- a/free_gait_core/CMakeLists.txt +++ b/free_gait_core/CMakeLists.txt @@ -94,6 +94,7 @@ add_library(${PROJECT_NAME} src/executor/StateBatch.cpp src/executor/StateBatchComputer.cpp src/executor/AdapterBase.cpp +# src/executor/AdapterDummy.cpp src/pose_optimization/PoseOptimizationBase.cpp src/pose_optimization/PoseConstraintsChecker.cpp src/pose_optimization/PoseOptimizationGeometric.cpp diff --git a/free_gait_core/include/free_gait_core/executor/executor.hpp.gch b/free_gait_core/include/free_gait_core/executor/executor.hpp.gch new file mode 100644 index 0000000..325db09 Binary files /dev/null and b/free_gait_core/include/free_gait_core/executor/executor.hpp.gch differ diff --git a/free_gait_core/package.xml b/free_gait_core/package.xml index 6c82edd..61da670 100644 --- a/free_gait_core/package.xml +++ b/free_gait_core/package.xml @@ -19,4 +19,9 @@ robot_utils std_utils message_logger + + + diff --git a/free_gait_core/plugin_description.xml b/free_gait_core/plugin_description.xml new file mode 100644 index 0000000..d8c73bd --- /dev/null +++ b/free_gait_core/plugin_description.xml @@ -0,0 +1,7 @@ + + + Display to preview Free Gait actions. + + diff --git a/free_gait_core/src/executor/BatchExecutor.cpp b/free_gait_core/src/executor/BatchExecutor.cpp index 63b771b..49c5941 100644 --- a/free_gait_core/src/executor/BatchExecutor.cpp +++ b/free_gait_core/src/executor/BatchExecutor.cpp @@ -73,7 +73,9 @@ void BatchExecutor::processInThread() while (!executor_.getQueue().empty() && !requestForCancelling_) { executor_.advance(timeStep_); time += timeStep_; +// std::cout<<"Updated Joint Positions: "<& limbs, const std::vector + +namespace free_gait { + +class AdapterDummy : public AdapterBase +{ + public: + AdapterDummy(); + virtual ~AdapterDummy(); + + //! Copying data from real robot to free gait state. + virtual bool resetExtrasWithRobot(const StepQueue& stepQueue, State& state); + virtual bool updateExtrasBefore(const StepQueue& stepQueue, State& state); + virtual bool updateExtrasAfter(const StepQueue& stepQueue, State& state); + + //! State independent functions. + virtual const std::string& getWorldFrameId() const; + virtual const std::string& getBaseFrameId() const; + virtual const std::vector& getLimbs() const; + virtual const std::vector& getBranches() const; + virtual LimbEnum getLimbEnumFromLimbString(const std::string& limb) const; + virtual std::string getLimbStringFromLimbEnum(const LimbEnum& limb) const; + virtual std::string getBaseString() const; + virtual JointNodeEnum getJointNodeEnumFromJointNodeString(const std::string& jointNode) const; + virtual std::string getJointNodeStringFromJointNodeEnum(const JointNodeEnum& jointNode) const; + virtual bool getLimbJointPositionsFromPositionBaseToFootInBaseFrame( + const Position& positionBaseToFootInBaseFrame, const LimbEnum& limb, + JointPositionsLeg& jointPositions) const; + virtual Position getPositionBaseToFootInBaseFrame(const LimbEnum& limb, + const JointPositionsLeg& jointPositions) const; + virtual Position getPositionBaseToHipInBaseFrame(const LimbEnum& limb) const; + + //! Reading state of real robot. + virtual bool isExecutionOk() const; + virtual bool isLegGrounded(const LimbEnum& limb) const; + virtual JointPositionsLeg getJointPositionsForLimb(const LimbEnum& limb) const; + virtual JointPositions getAllJointPositions() const; + virtual JointVelocitiesLeg getJointVelocitiesForLimb(const LimbEnum& limb) const; + virtual JointVelocities getAllJointVelocities() const; + virtual JointAccelerationsLeg getJointAccelerationsForLimb(const LimbEnum& limb) const; + virtual JointAccelerations getAllJointAccelerations() const; + virtual JointEffortsLeg getJointEffortsForLimb(const LimbEnum& limb) const; + virtual JointEfforts getAllJointEfforts() const; + virtual Position getPositionWorldToBaseInWorldFrame() const; + virtual RotationQuaternion getOrientationBaseToWorld() const; + virtual LinearVelocity getLinearVelocityBaseInWorldFrame() const; + virtual LocalAngularVelocity getAngularVelocityBaseInBaseFrame() const; + virtual LinearAcceleration getLinearAccelerationBaseInWorldFrame() const; + virtual AngularAcceleration getAngularAccelerationBaseInBaseFrame() const; + virtual Position getPositionBaseToFootInBaseFrame(const LimbEnum& limb) const; + virtual Position getPositionWorldToFootInWorldFrame(const LimbEnum& limb) const; + virtual Position getCenterOfMassInWorldFrame() const; + + /*! + * Transform is frameId to world => C_IF. + * @param frameId + * @return + */ + virtual void getAvailableFrameTransforms(std::vector& frameTransforms) const; + virtual Pose getFrameTransform(const std::string& frameId) const; + virtual ControlSetup getControlSetup(const BranchEnum& branch) const; + virtual ControlSetup getControlSetup(const LimbEnum& limb) const; + + //! Depending on state of real robot. + virtual JointVelocitiesLeg getJointVelocitiesFromEndEffectorLinearVelocityInWorldFrame( + const LimbEnum& limb, const LinearVelocity& endEffectorLinearVelocityInWorldFrame) const; + virtual LinearVelocity getEndEffectorLinearVelocityFromJointVelocities(const LimbEnum& limb, + const JointVelocitiesLeg& jointVelocities, + const std::string& frameId) const; + virtual JointAccelerationsLeg getJointAccelerationsFromEndEffectorLinearAccelerationInWorldFrame( + const LimbEnum& limb, const LinearAcceleration& endEffectorLinearAccelerationInWorldFrame) const; + + //! Hook to write data to internal robot representation from state. +// virtual bool setInternalDataFromState(const State& state) const; + virtual bool setInternalDataFromState(const State& state, bool updateContacts = true, bool updatePosition = true, + bool updateVelocity = true, bool updateAcceleration = false) const; + + virtual void createCopyOfState() const; + virtual void resetToCopyOfState() const; + + const State& getState() const; + + private: + std::unique_ptr state_; + std::vector limbs_; + std::vector branches_; + const std::string worldFrameId_; + const std::string baseFrameId_; + Stance footholdsInSupport_; +}; + +} /* namespace free_gait */ diff --git a/free_gait_ros/include/free_gait_ros/AdapterRos.hpp b/free_gait_ros/include/free_gait_ros/AdapterRos.hpp index 5a62b45..7663f30 100644 --- a/free_gait_ros/include/free_gait_ros/AdapterRos.hpp +++ b/free_gait_ros/include/free_gait_ros/AdapterRos.hpp @@ -12,6 +12,7 @@ #include #include #include +#include "pluginlib_tutorials/polygon_base.h" // STD #include @@ -39,6 +40,8 @@ class AdapterRos private: ros::NodeHandle& nodeHandle_; + pluginlib::ClassLoader tutor_loader_; + std::unique_ptr tutor_; pluginlib::ClassLoader adapterLoader_; std::unique_ptr adapter_; pluginlib::ClassLoader adapterRosInterfaceLoader_; diff --git a/free_gait_ros/include/free_gait_ros/AdapterRosInterfaceBase.hpp b/free_gait_ros/include/free_gait_ros/AdapterRosInterfaceBase.hpp index 6f8059e..304a0c7 100644 --- a/free_gait_ros/include/free_gait_ros/AdapterRosInterfaceBase.hpp +++ b/free_gait_ros/include/free_gait_ros/AdapterRosInterfaceBase.hpp @@ -36,13 +36,13 @@ class AdapterRosInterfaceBase virtual bool initializeAdapter(AdapterBase& adapter) const = 0; virtual bool updateAdapterWithRobotState(AdapterBase& adapter) const = 0; - void updateRobotState(const free_gait_msgs::RobotStateConstPtr& robotState); +// void updateRobotState(const free_gait_msgs::RobotStateConstPtr& robotState); protected: ros::NodeHandle* nodeHandle_; std::string robotDescriptionUrdfString_; - ros::Subscriber joint_states_sub_; - JointPositions all_joint_positions_; +// ros::Subscriber joint_states_sub_; +// JointPositions all_joint_positions_; }; diff --git a/free_gait_ros/include/free_gait_ros/AdapterRosInterfaceDummy.hpp b/free_gait_ros/include/free_gait_ros/AdapterRosInterfaceDummy.hpp new file mode 100644 index 0000000..6cd4a2f --- /dev/null +++ b/free_gait_ros/include/free_gait_ros/AdapterRosInterfaceDummy.hpp @@ -0,0 +1,40 @@ +/* + * filename.hpp + * Descriotion: + * + * Created on: Dec 26, 2018 + * Author: Shunyao Wang + * Institute: Harbin Institute of Technology, Shenzhen + */ + +#pragma once +#include "moveit_msgs/RobotState.h" +#include "free_gait_ros/free_gait_ros.hpp" + +namespace free_gait { + +class AdapterRosInterfaceDummy : public AdapterRosInterfaceBase +{ +public: + AdapterRosInterfaceDummy(); + ~AdapterRosInterfaceDummy(); + + virtual bool subscribeToRobotState(const std::string& robotStateTopic); + virtual void unsubscribeFromRobotState(); + virtual const std::string getRobotStateMessageType(); + virtual bool isReady() const; + + //! Update adapter. + virtual bool initializeAdapter(AdapterBase& adapter) const; + virtual bool updateAdapterWithRobotState(AdapterBase& adapter) const; + + void updateRobotState(const free_gait_msgs::RobotStateConstPtr& robotState); + + +private: + ros::Subscriber joint_states_sub_; + JointPositions all_joint_positions_; + +}; + +} // namespace diff --git a/free_gait_ros/include/free_gait_ros/free_gait_ros.hpp b/free_gait_ros/include/free_gait_ros/free_gait_ros.hpp index 2113ae8..d0933f3 100644 --- a/free_gait_ros/include/free_gait_ros/free_gait_ros.hpp +++ b/free_gait_ros/include/free_gait_ros/free_gait_ros.hpp @@ -17,3 +17,4 @@ #include "free_gait_ros/StepFrameConverter.hpp" #include "free_gait_ros/StateRosPublisher.hpp" #include "free_gait_ros/RosVisualization.hpp" +#include "free_gait_ros/AdapterDummy.hpp" diff --git a/free_gait_ros/launch/test.launch b/free_gait_ros/launch/test.launch new file mode 100644 index 0000000..fbc63d2 --- /dev/null +++ b/free_gait_ros/launch/test.launch @@ -0,0 +1,8 @@ + + + + + + + diff --git a/free_gait_ros/package.xml b/free_gait_ros/package.xml index ae4e5f1..08416f9 100644 --- a/free_gait_ros/package.xml +++ b/free_gait_ros/package.xml @@ -22,4 +22,8 @@ robot_state_publisher kdl_parser eigen + + + + diff --git a/free_gait_ros/plugin_description.xml b/free_gait_ros/plugin_description.xml new file mode 100644 index 0000000..876334a --- /dev/null +++ b/free_gait_ros/plugin_description.xml @@ -0,0 +1,12 @@ + + + Display to preview Free Gait actions. + + + Display to preview Free Gait actions. + + diff --git a/free_gait_ros/src/AdapterDummy.cpp b/free_gait_ros/src/AdapterDummy.cpp new file mode 100644 index 0000000..de90444 --- /dev/null +++ b/free_gait_ros/src/AdapterDummy.cpp @@ -0,0 +1,407 @@ +/* + * AdapterDummy.cpp + * + * Created on: Mar 23, 2017 + * Author: Péter Fankhauser + * Institute: ETH Zurich + */ + +#include "free_gait_ros/AdapterDummy.hpp" + +namespace free_gait { + +AdapterDummy::AdapterDummy() + : AdapterBase(), + worldFrameId_("odom"), + baseFrameId_("base_link") +{ + state_.reset(new State()); + state_->initialize(getLimbs(), getBranches()); +// state_->LoadRobotDescriptionFromFile("/home/hitstar/catkin_ws/src/quadruped_locomotion-dev/quadruped_model/urdf/simpledog.urdf"); +// std::cout<LF_Chain.getNrOfJoints()<getPositionWorldToBaseInWorldFrame(); + RotationQuaternion I_R_B = state_->getOrientationBaseToWorld(); +// BaseMotionBase baseMotion = stepQueue.getCurrentStep().getBaseMotion(); +// BaseAuto baseAuto; +// BaseTarget baseTarget; +// BaseTrajectory baseTrajectory; +// switch (baseMotion.getType()) { +// case BaseMotionBase::Type::Auto: +// { +// baseAuto = (dynamic_cast(baseMotion)); +// break; +// } +// case BaseMotionBase::Type::Target: +// { +// baseTarget = (dynamic_cast(baseMotion)); +// break; +// } +// case BaseMotionBase::Type::Trajectory: +// { +// baseTrajectory = (dynamic_cast(baseMotion)); +// break; +// } +// default: +// { +// baseAuto = (dynamic_cast(baseMotion)); +// break; +// } +// } + if(stepQueue.empty()) return true; + double time = stepQueue.getCurrentStep().getTime(); +// std::cout<<"+++++++++++++TIME++++++++++++++++"<isSupportLeg(limb)) + { + if(!(time>0.001)) + { + footholdsInSupport_[limb] = state_->getPositionWorldToFootInWorldFrame(limb); + } +// I_r_IF = state_->getPositionWorldToFootInWorldFrame(limb); + I_r_IF = footholdsInSupport_.at(limb); +// baseMotion.getDuration(); +// std::cout<<"=========================="<getLimbJointPositionsFromPositionBaseToFootInBaseFrame(B_r_BF, limb, joint_limb); + state_->setJointPositionsForLimb(limb, joint_limb); + } else { + + } + } +// stepQueue.getCurrentStep().getLegMotion(LimbEnum::LF_LEG + return true; +} + +const std::string& AdapterDummy::getWorldFrameId() const +{ + return worldFrameId_; +} + +const std::string& AdapterDummy::getBaseFrameId() const +{ + return baseFrameId_; +} + +const std::vector& AdapterDummy::getLimbs() const +{ + return limbs_; +} + +const std::vector& AdapterDummy::getBranches() const +{ + return branches_; +} + +LimbEnum AdapterDummy::getLimbEnumFromLimbString(const std::string& limb) const +{ + if(limb == "LF_LEG") + return static_cast(0); + if(limb == "RF_LEG") + return static_cast(1); + if(limb == "LH_LEG") + return static_cast(2); + if(limb == "RH_LEG") + return static_cast(3); + +// throw std::runtime_error("AdapterDummy::getLimbEnumFromLimbString() is not implemented."); +} + +std::string AdapterDummy::getLimbStringFromLimbEnum(const LimbEnum& limb) const +{ + if(limb == LimbEnum::LF_LEG) + return "LF_LEG"; + if(limb == LimbEnum::RF_LEG) + return "RF_LEG"; + if(limb == LimbEnum::LH_LEG) + return "LH_LEG"; + if(limb == LimbEnum::RH_LEG) + return "RH_LEG"; +// throw std::runtime_error("AdapterDummy::getLimbStringFromLimbEnum() is not implemented."); +} + +std::string AdapterDummy::getBaseString() const +{ + return "BASE"; + throw std::runtime_error("AdapterDummy::getBaseString() is not implemented."); +} + +JointNodeEnum AdapterDummy::getJointNodeEnumFromJointNodeString(const std::string& jointNode) const +{ + throw std::runtime_error("AdapterDummy::getJointNodeEnumFromJointNodeString() is not implemented."); +} + +std::string AdapterDummy::getJointNodeStringFromJointNodeEnum(const JointNodeEnum& jointNode) const +{ + throw std::runtime_error("AdapterDummy::getJointNodeStringFromJointNodeEnum() is not implemented."); +} + +bool AdapterDummy::getLimbJointPositionsFromPositionBaseToFootInBaseFrame( + const Position& positionBaseToFootInBaseFrame, const LimbEnum& limb, + JointPositionsLeg& jointPositions) const +{ + // TODO(Shunyao): solve the single limb IK +// QuadrupedKinematics::FowardKinematicsSolve() +// state_->FowardKinematicsSolve() + std::cout<<"start"<getLimbJointPositionsFromPositionBaseToFootInBaseFrame(positionBaseToFootInBaseFrame, + limb,jointPositions)) + { + return true; + } + throw std::runtime_error("AdapterDummy::getLimbJointPositionsFromPositionBaseToFootInBaseFrame() is not implemented."); +} + +Position AdapterDummy::getPositionBaseToFootInBaseFrame( + const LimbEnum& limb, const JointPositionsLeg& jointPositions) const +{ + // TODO(Shunyao): solve the single limb Kinematics +// state_->FowardKinematicsSolve() + return state_->getPositionBaseToFootInBaseFrame(limb,jointPositions); + throw std::runtime_error("AdapterDummy::getPositionBaseToFootInBaseFrame() is not implemented."); +} + +Position AdapterDummy::getPositionBaseToHipInBaseFrame(const LimbEnum& limb) const +{ + switch (limb) { + case LimbEnum::LF_LEG: + return Position(0.3, 0.2, 0.0); + case LimbEnum::RF_LEG: + return Position(0.3, -0.2, 0.0); + case LimbEnum::LH_LEG: + return Position(-0.3, 0.2, 0.0); + case LimbEnum::RH_LEG: + return Position(-0.3, -0.2, 0.0); + default: + throw std::runtime_error("AdapterDummy::getPositionBaseToHipInBaseFrame() something went wrong."); + } +} + +bool AdapterDummy::isExecutionOk() const +{ + // TODO(Shunyao): How to confirm isOK? from State? + return true; + throw std::runtime_error("AdapterDummy::isExecutionOk() is not implemented."); +} + +bool AdapterDummy::isLegGrounded(const LimbEnum& limb) const +{ +// return state_->isSupportLeg(limb); + return true; + throw std::runtime_error("AdapterDummy::isLegGrounded() is not implemented."); +} + +JointPositionsLeg AdapterDummy::getJointPositionsForLimb(const LimbEnum& limb) const +{ + return state_->getJointPositionsForLimb(limb); +} + +JointPositions AdapterDummy::getAllJointPositions() const +{ + return state_->getJointPositions(); +} + +JointVelocitiesLeg AdapterDummy::getJointVelocitiesForLimb(const LimbEnum& limb) const +{ + return state_->getJointVelocitiesForLimb(limb); +} + +JointVelocities AdapterDummy::getAllJointVelocities() const +{ + return state_->getJointVelocities(); +} + +JointAccelerationsLeg AdapterDummy::getJointAccelerationsForLimb(const LimbEnum& limb) const +{ + throw std::runtime_error("AdapterDummy::getJointAccelerationsForLimb() is not implemented."); +} + +JointAccelerations AdapterDummy::getAllJointAccelerations() const +{ + throw std::runtime_error("AdapterDummy::getAllJointAccelerations() is not implemented."); +} + +JointEffortsLeg AdapterDummy::getJointEffortsForLimb(const LimbEnum& limb) const +{ + return state_->getJointEffortsForLimb(limb); +} + +JointEfforts AdapterDummy::getAllJointEfforts() const +{ + return state_->getAllJointEfforts(); +} + +Position AdapterDummy::getPositionWorldToBaseInWorldFrame() const +{ +// return Position(0,0,0); + return state_->getPositionWorldToBaseInWorldFrame(); +} + +RotationQuaternion AdapterDummy::getOrientationBaseToWorld() const +{ +// return RotationQuaternion(1,0,0,0); + return state_->getOrientationBaseToWorld(); +} + +LinearVelocity AdapterDummy::getLinearVelocityBaseInWorldFrame() const +{ + return state_->getLinearVelocityBaseInWorldFrame(); +} + +LocalAngularVelocity AdapterDummy::getAngularVelocityBaseInBaseFrame() const +{ + return state_->getAngularVelocityBaseInBaseFrame(); +} + +LinearAcceleration AdapterDummy::getLinearAccelerationBaseInWorldFrame() const +{ + throw std::runtime_error("AdapterDummy::getLinearAccelerationBaseInWorldFrame() is not implemented."); +} + +AngularAcceleration AdapterDummy::getAngularAccelerationBaseInBaseFrame() const +{ + throw std::runtime_error("AdapterDummy::getAngularAccelerationBaseInBaseFrame() is not implemented."); +} + +Position AdapterDummy::getPositionBaseToFootInBaseFrame(const LimbEnum& limb) const +{ + return state_->getPositionBaseToFootInBaseFrame(limb); + throw std::runtime_error("AdapterDummy::getPositionBaseToFootInBaseFrame() is not implemented."); +} + +Position AdapterDummy::getPositionWorldToFootInWorldFrame(const LimbEnum& limb) const +{ + return state_->getPositionWorldToFootInWorldFrame(limb); + throw std::runtime_error("AdapterDummy::getPositionWorldToFootInWorldFrame() is not implemented."); +} + +Position AdapterDummy::getCenterOfMassInWorldFrame() const +{ +// return Position(0,0,0); + return state_->getPositionWorldToBaseInWorldFrame(); +} + +void AdapterDummy::getAvailableFrameTransforms(std::vector& frameTransforms) const +{ + frameTransforms.push_back("/odom"); +// throw std::runtime_error("AdapterDummy::getAvailableFrameTransforms() is not implemented."); +} + +Pose AdapterDummy::getFrameTransform(const std::string& frameId) const +{ + //TODO(Shunyao): It's seems to feedback Odom To Map, amcl? + return Pose(Position(0,0,0),RotationQuaternion());//fixed odom with map temparently + throw std::runtime_error("AdapterDummy::getFrameTransform() is not implemented."); +} + +ControlSetup AdapterDummy::getControlSetup(const BranchEnum& branch) const +{ +// TODO(Shunyao): How to set up Control Level, In State class to set controlSetup + return ControlSetup({{ControlLevel::Position, true}, + {ControlLevel::Velocity, false}, + {ControlLevel::Acceleration, false}, + {ControlLevel::Effort, false}}); + throw std::runtime_error("AdapterDummy::getControlSetup() is not implemented."); +} + +ControlSetup AdapterDummy::getControlSetup(const LimbEnum& limb) const +{ + return ControlSetup({{ControlLevel::Position, true}, + {ControlLevel::Velocity, false}, + {ControlLevel::Acceleration, false}, + {ControlLevel::Effort, false}}); + throw std::runtime_error("AdapterDummy::getControlSetup() is not implemented."); +} + +//! State depending on real robot. +JointVelocitiesLeg AdapterDummy::getJointVelocitiesFromEndEffectorLinearVelocityInWorldFrame( + const LimbEnum& limb, const LinearVelocity& endEffectorLinearVelocityInWorldFrame) const +{ + throw std::runtime_error("AdapterDummy::getJointVelocitiesFromEndEffectorLinearVelocityInWorldFrame() is not implemented."); +} + +LinearVelocity AdapterDummy::getEndEffectorLinearVelocityFromJointVelocities(const LimbEnum& limb, + const JointVelocitiesLeg& jointVelocities, + const std::string& frameId) const +{ + //TODO(Shunyao): dp = Jacobian * dq +// if(frameId != getBaseFrameId()) +// { +// transformLinearVelocity(frameId, getBaseFrameId(),) +// } + return state_->getEndEffectorLinearVelocityFromJointVelocities(limb, jointVelocities); + throw std::runtime_error("AdapterDummy::getEndEffectorLinearVelocityFromJointVelocities() is not implemented."); +} + +JointAccelerationsLeg AdapterDummy::getJointAccelerationsFromEndEffectorLinearAccelerationInWorldFrame( + const LimbEnum& limb, const LinearAcceleration& endEffectorLinearAccelerationInWorldFrame) const +{ + throw std::runtime_error("AdapterDummy::getJointAccelerationsFromEndEffectorLinearAccelerationInWorldFrame() is not implemented."); +} + +bool AdapterDummy::setInternalDataFromState(const State& state, bool updateContacts, bool updatePosition, + bool updateVelocity, bool updateAcceleration) const +{ + *state_ = state; + return true; +} + +void AdapterDummy::createCopyOfState() const +{ + +} + +void AdapterDummy::resetToCopyOfState() const +{ + +} + +const State& AdapterDummy::getState() const +{ + return *state_; +} + +} /* namespace */ + +#include +PLUGINLIB_EXPORT_CLASS(free_gait::AdapterDummy, free_gait::AdapterBase) diff --git a/free_gait_ros/src/AdapterRos.cpp b/free_gait_ros/src/AdapterRos.cpp index eb8b7ec..78b219b 100644 --- a/free_gait_ros/src/AdapterRos.cpp +++ b/free_gait_ros/src/AdapterRos.cpp @@ -7,28 +7,47 @@ */ #include "free_gait_ros/AdapterRos.hpp" - namespace free_gait { AdapterRos::AdapterRos(ros::NodeHandle& nodeHandle, const AdapterType type) : nodeHandle_(nodeHandle), - adapterLoader_("free_gait_core", "free_gait::AdapterBase"), + tutor_loader_("pluginlib_tutorials", "polygon_base::RegularPolygon"), + adapterLoader_("free_gait_ros", "free_gait::AdapterBase"), adapterRosInterfaceLoader_("free_gait_ros", "free_gait::AdapterRosInterfaceBase") { // Load and initialize adapter. + std::cout<<"Constructing AdapterRos..."< tutor_loader("pluginlib_tutorials", "polygon_base::RegularPolygon"); +// tutor_loader.createInstance("pluginlib_tutorials/regular_triangle"); + tutor_.reset(tutor_loader_.createUnmanagedInstance("pluginlib_tutorials/regular_triangle")); + std::cout<<"Haved loaded tutorials"< libs_before = adapterRosInterfaceLoader_.getRegisteredLibraries(); std::string adapterRosInterfacePluginName; nodeHandle.getParam("/free_gait/adapter_ros_interface_plugin", adapterRosInterfacePluginName); + std::cout<<"===="< libs_after =adapterRosInterfaceLoader_.getRegisteredLibraries(); + std::cout<setNodeHandle(nodeHandle_); adapterRosInterface_->readRobotDescription(); diff --git a/free_gait_ros/src/AdapterRosInterfaceBase.cpp b/free_gait_ros/src/AdapterRosInterfaceBase.cpp index d193c70..f83f6cf 100644 --- a/free_gait_ros/src/AdapterRosInterfaceBase.cpp +++ b/free_gait_ros/src/AdapterRosInterfaceBase.cpp @@ -10,6 +10,7 @@ #include + namespace free_gait { AdapterRosInterfaceBase::AdapterRosInterfaceBase() @@ -34,13 +35,14 @@ bool AdapterRosInterfaceBase::readRobotDescription() } else { throw pluginlib::PluginlibException("Did not find ROS parameter for robot description '/free_gait/robot_description'."); } - - if (nodeHandle_->hasParam(robotDescriptionPath)) { - nodeHandle_->getParam(robotDescriptionPath, robotDescriptionUrdfString_); - } else { - throw pluginlib::PluginlibException("Did not find ROS parameter for robot description '" + robotDescriptionPath + "'."); - } - + robotDescriptionUrdfString_ = robotDescriptionPath; +// ROS_INFO("================================================="); +// if (nodeHandle_->hasParam(robotDescriptionPath)) { +// nodeHandle_->getParam(robotDescriptionPath, robotDescriptionUrdfString_); +// } else { +// throw pluginlib::PluginlibException("Did not find ROS parameter for robot description '" + robotDescriptionPath + "'."); +// } +// ROS_INFO("================================================="); return true; } bool AdapterRosInterfaceBase::updateAdapterWithRobotState(AdapterBase& adapter) const @@ -54,17 +56,17 @@ bool AdapterRosInterfaceBase::updateAdapterWithRobotState(AdapterBase& adapter) bool AdapterRosInterfaceBase::subscribeToRobotState(const std::string& robotStateTopic) { // topic: "/free_gait/robot_state", - joint_states_sub_ = nodeHandle_->subscribe("/robot_state", 1, &AdapterRosInterfaceBase::updateRobotState,this); + //joint_states_sub_ = nodeHandle_->subscribe("/robot_state", 1, &AdapterRosInterfaceBase::updateRobotState,this); } -void AdapterRosInterfaceBase::updateRobotState(const free_gait_msgs::RobotStateConstPtr& robotState) -{ - for(int i = 1;i<12;i++) - { - all_joint_positions_(i) = robotState->lf_leg_joints.position[i]; - } +//void AdapterRosInterfaceBase::updateRobotState(const free_gait_msgs::RobotStateConstPtr& robotState) +//{ +// for(int i = 1;i<12;i++) +// { +// all_joint_positions_(i) = robotState->lf_leg_joints.position[i]; +// } -} +//} } /* namespace free_gait */ diff --git a/free_gait_ros/src/AdapterRosInterfaceDummy.cpp b/free_gait_ros/src/AdapterRosInterfaceDummy.cpp new file mode 100644 index 0000000..7a16295 --- /dev/null +++ b/free_gait_ros/src/AdapterRosInterfaceDummy.cpp @@ -0,0 +1,76 @@ +/* + * AdapterRosInterfaceDummy.cpp + * Descriotion: Adapter Ros interface with Dummy states, use for a visulization + * in Rviz + * + * Created on: Dec 26, 2018 + * Author: Shunyao Wang + * Institute: Harbin Institute of Technology, Shenzhen + */ + +#include "free_gait_ros/AdapterRosInterfaceDummy.hpp" + +namespace free_gait { + +AdapterRosInterfaceDummy::AdapterRosInterfaceDummy() +{ + +} + +AdapterRosInterfaceDummy::~AdapterRosInterfaceDummy() +{ + +} + +bool AdapterRosInterfaceDummy::subscribeToRobotState(const std::string& robotStateTopic) +{ + joint_states_sub_ = nodeHandle_->subscribe(robotStateTopic, 1, &AdapterRosInterfaceDummy::updateRobotState,this); +} +void AdapterRosInterfaceDummy::unsubscribeFromRobotState() +{ + joint_states_sub_.shutdown(); +// throw std::runtime_error("AdapterRosInterfaceDummy::unsubscribeFromRobotState() is not implemented."); + +} +const std::string AdapterRosInterfaceDummy::getRobotStateMessageType() +{ + return ros::message_traits::datatype(); + throw std::runtime_error("AdapterRosInterfaceDummy::getRobotStateMessageType() is not implemented."); + +} +bool AdapterRosInterfaceDummy::isReady() const +{ + throw std::runtime_error("AdapterRosInterfaceDummy::isReady() is not implemented."); + +} + + +//! Update adapter. +bool AdapterRosInterfaceDummy::initializeAdapter(AdapterBase& adapter) const +{ + std::cout<<"Initial Adapter"<lf_leg_joints.position[i]; + } + +} + +}//namespace + + +#include +PLUGINLIB_EXPORT_CLASS(free_gait::AdapterRosInterfaceDummy, free_gait::AdapterRosInterfaceBase) diff --git a/free_gait_ros/src/FreeGaitActionServer.cpp b/free_gait_ros/src/FreeGaitActionServer.cpp index 88d7bd3..69cae38 100644 --- a/free_gait_ros/src/FreeGaitActionServer.cpp +++ b/free_gait_ros/src/FreeGaitActionServer.cpp @@ -72,6 +72,7 @@ void FreeGaitActionServer::update() } } else { // Ongoing. + ROS_INFO("publish feedback"); lock.lock(); if (executor_.getQueue().active()) publishFeedback(); lock.unlock(); diff --git a/free_gait_ros/src/StateRosPublisher.cpp b/free_gait_ros/src/StateRosPublisher.cpp index c7b742c..5455f00 100644 --- a/free_gait_ros/src/StateRosPublisher.cpp +++ b/free_gait_ros/src/StateRosPublisher.cpp @@ -85,7 +85,7 @@ bool StateRosPublisher::publish(const State& state) state.getAllJointNames(jointNames); - JointPositions jointPositions = state.getJointPositions(); + JointPositions jointPositions = state.getJointPositionsToReach();//.getJointPositions(); if (jointNames.size() != jointPositions.vector().size()) { ROS_ERROR("Joint name vector and joint position are not of equal size!"); @@ -94,12 +94,12 @@ bool StateRosPublisher::publish(const State& state) std::map jointPositionsMap; for (size_t i = 0; i < jointNames.size(); ++i) { jointPositionsMap[jointNames[i]] = jointPositions(i); - std::cout<publishTransforms(jointPositionsMap, time, tfPrefix_); robotStatePublisher_->publishFixedTransforms(tfPrefix_); -ROS_INFO("In ros state publisher"); +//ROS_INFO("In ros state publisher"); // Publish base position. geometry_msgs::TransformStamped tfTransform; tfTransform.header.stamp = time; diff --git a/free_gait_ros/test/action_server_test.cpp b/free_gait_ros/test/action_server_test.cpp new file mode 100644 index 0000000..f03be3e --- /dev/null +++ b/free_gait_ros/test/action_server_test.cpp @@ -0,0 +1,105 @@ +/* + * action_server_test.cpp + * Descriotion: + * + * Created on: Aug 31, 2017 + * Author: Shunyao Wang + * Institute: Harbin Institute of Technology, Shenzhen + */ +#include "free_gait_ros/free_gait_ros.hpp" +#include "free_gait_core/free_gait_core.hpp" +#include "pluginlib/class_loader.h" +#include "boost/bind.hpp" +#include "boost/thread.hpp" + +using namespace free_gait; +using namespace std; +class ActionServerTest +{ +public: + ActionServerTest(ros::NodeHandle& nodehandle) + : nodeHandle_(nodehandle), + adapter_loader("free_gait_ros", "free_gait::AdapterBase") + { + adapter.reset(adapter_loader.createUnmanagedInstance("free_gait_ros/AdapterDummy")); + state.reset(new State()); + parameters.reset(new StepParameters()); + completer.reset(new StepCompleter(*parameters, *adapter)); + computer.reset(new StepComputer()); + executor.reset(new Executor(*completer, *computer, *adapter, *state)); + rosPublisher.reset(new StateRosPublisher(nodeHandle_, *adapter)); + executor->initialize(); + + server_.reset(new FreeGaitActionServer(nodeHandle_, "/free_gait/action_server", *executor, *adapter)); + server_->initialize(); + server_->start(); +// server_->update(); + action_server_thread_ = boost::thread(boost::bind(&ActionServerTest::ActionServerThread, this)); + + } + ~ActionServerTest() {} + + void ActionServerThread()//(const FreeGaitActionServer& server) + { + ROS_INFO("In action server thread"); + double dt = 0.001; + double time = 0.0; + ros::Rate rate(100); + while (ros::ok()) { + server_->update(); +// TODO(Shunyao): How to Update? + if (!executor->getQueue().empty()) { + executor->advance(dt, true); + time = time + dt; + rosPublisher->publish(adapter->getState()); + } + rate.sleep(); + } + + } +private: +// FreeGaitActionServer server_; + std::unique_ptr server_; + boost::thread action_server_thread_; + ros::NodeHandle nodeHandle_; + std::unique_ptr adapter; + pluginlib::ClassLoader adapter_loader; + std::unique_ptr state; + std::unique_ptr parameters; + std::unique_ptr completer; + std::unique_ptr computer; + std::unique_ptr executor; + std::unique_ptr rosPublisher; +}; + + + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "action_server_test"); + ros::NodeHandle nh("~"); + ActionServerTest ac_test(nh); + ros::spin(); +// pluginlib::ClassLoader adapter_loader("free_gait_ros", "free_gait::AdapterBase"); +// std::unique_ptr adapter; +// adapter.reset(adapter_loader.createUnmanagedInstance("free_gait_ros/AdapterDummy")); + + +// double dt = 0.001; +// double time = 0.0; +//// ros::Time t_start = ros::Time::now(); +// ros::Rate rate(1000); +// ros::spin(); + +// while (!executor.getQueue().empty()) { +// executor.advance(dt, true); +// time = time + dt; +// rosPublisher.publish(adapter->getState()); + +// } + +// ros::Time t_end = ros::Time::now(); +// cout<<"end time : "< adapter_loader("free_gait_ros", "free_gait::AdapterBase"); + std::unique_ptr adapter; + adapter.reset(adapter_loader.createUnmanagedInstance("free_gait_ros/AdapterDummy")); + +// const AdapterDummy& adapter = dynamic_cast(adapterRos_.getAdapter()); +// AdapterDummy adapter; +// AdapterBase& adapter = adapterRos_.getAdapter(); JointPositionsLeg joints(0,0,0); // Position end(0.4,0.25,-0.5); // State testState; // testState.getPositionBaseToFootInBaseFrame(LimbEnum::LF_LEG,joints); - Position end = adapter.getPositionBaseToFootInBaseFrame(LimbEnum::LF_LEG,joints); + Position end = adapter->getPositionBaseToFootInBaseFrame(LimbEnum::LF_LEG,joints); cout<getLimbJointPositionsFromPositionBaseToFootInBaseFrame(end,LimbEnum::LF_LEG,joints); cout< steps; Step step; @@ -169,14 +178,14 @@ int main(int argc, char **argv) executor.getQueue().add(steps); executor.setPreemptionType(Executor::PreemptionType::PREEMPT_IMMEDIATE); - double dt = 0.01; + double dt = 0.001; double time = 0.0; ros::Time t_start = ros::Time::now(); - ros::Rate rate(100); + ros::Rate rate(1000); while (!executor.getQueue().empty()) { executor.advance(dt, true); time = time + dt; - rosPublisher.publish(adapter.getState()); + rosPublisher.publish(adapter->getState()); rate.sleep(); } ros::Time t_end = ros::Time::now(); diff --git a/free_gait_rviz_plugin/CMakeLists.txt b/free_gait_rviz_plugin/CMakeLists.txt index c98d0b3..ba27558 100644 --- a/free_gait_rviz_plugin/CMakeLists.txt +++ b/free_gait_rviz_plugin/CMakeLists.txt @@ -107,7 +107,11 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} rviz_free_gait_additions ) +add_executable(class_load_test test/class_load_test.cpp) +target_link_libraries(class_load_test + ${PROJECT_NAME} + ) ## Install rules install( DIRECTORY include/${PROJECT_NAME} diff --git a/free_gait_rviz_plugin/include/free_gait_rviz_plugin/FreeGaitPreviewDisplay.hpp b/free_gait_rviz_plugin/include/free_gait_rviz_plugin/FreeGaitPreviewDisplay.hpp index 0677442..953e23f 100644 --- a/free_gait_rviz_plugin/include/free_gait_rviz_plugin/FreeGaitPreviewDisplay.hpp +++ b/free_gait_rviz_plugin/include/free_gait_rviz_plugin/FreeGaitPreviewDisplay.hpp @@ -14,7 +14,7 @@ #include #include - +//#include "free_gait_core/executor/AdapterDummy.hpp" #include #include #endif diff --git a/free_gait_rviz_plugin/src/FreeGaitPreviewDisplay.cpp b/free_gait_rviz_plugin/src/FreeGaitPreviewDisplay.cpp index 00bd0f2..5b5c865 100644 --- a/free_gait_rviz_plugin/src/FreeGaitPreviewDisplay.cpp +++ b/free_gait_rviz_plugin/src/FreeGaitPreviewDisplay.cpp @@ -444,9 +444,21 @@ void FreeGaitPreviewDisplay::processMessage(const free_gait_msgs::ExecuteStepsAc adapterRos_.updateAdapterWithState(); } else if (startStateMethodProperty_->getOptionInt() == StartStateMethod::ContinuePreviewedState) { double time; - bool success = playback_.getStateBatch().getEndTimeOfStep(feedbackMessage_.feedback.step_id, time); +// bool success = playback_.getStateBatch().getEndTimeOfStep(feedbackMessage_.feedback.step_id, time); + bool success = true; + if(!playback_.getStateBatch().getStates().empty()){ + time = playback_.getStateBatch().getEndTime(); + }else{ + time = 0; + success = false; + } +// if (success) { if (success) { adapterRos_.getAdapter().setInternalDataFromState(playback_.getStateBatch().getState(time)); +// ROS_DEBUG("FreeGaitPreviewDisplay::processMessage: %f",adapterRos_.getAdapter().getPositionWorldToBaseInWorldFrame()(2)); + std::cout<<"=================FreeGaitPreviewDisplay::processMessage=================" + <getStateBatch(); // Deep copy. +// std::cout<<"======================================="< tutor_loader("pluginlib_tutorials", "polygon_base::RegularPolygon"); + tutor_loader.createInstance("pluginlib_tutorials/regular_triangle"); + // AdapterRos adapterRos_(nh, free_gait::AdapterRos::AdapterType::Preview); +// pluginlib::ClassLoader adapterLoader("free_gait_core", "free_gait::AdapterBase"); + pluginlib::ClassLoader* display_loader; + display_loader = new pluginlib::ClassLoader("rviz", "rviz::Display"); +// std::unique_ptr adapter_loader; +// std::unique_ptr display; +// display.reset(display_loader.createUnmanagedInstance("free_gait_rviz_plugin/FreeGaitPreviewDisplay")); +// adapter_loader.reset(adapterLoader.createUnmanagedInstance("free_gait::AdapterDummy")); + display_loader->createUnmanagedInstance("free_gait_rviz_plugin/FreeGaitPreview"); + return 0; +} diff --git a/my_actions/CMakeLists.txt b/my_actions/CMakeLists.txt new file mode 100644 index 0000000..688f0d3 --- /dev/null +++ b/my_actions/CMakeLists.txt @@ -0,0 +1,199 @@ +cmake_minimum_required(VERSION 2.8.3) +project(my_actions) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + free_gait_action_loader + roscpp + rospy +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES my_actions +# CATKIN_DEPENDS free_gait_action_loader roscpp rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/my_actions.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/my_actions_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_my_actions.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/my_actions/actions/test.yaml b/my_actions/actions/test.yaml new file mode 100644 index 0000000..caf0833 --- /dev/null +++ b/my_actions/actions/test.yaml @@ -0,0 +1,13 @@ +actions: + - action: + id: "1" + name: stand up + file: motion_scripts/standup.yaml + type: yaml + description: robot stand + - action: + id: "2" + name: sit down + file: motion_scripts/sitdown.yaml + type: yaml + description: robot sit down diff --git a/my_actions/collections/test_collections.yaml b/my_actions/collections/test_collections.yaml new file mode 100644 index 0000000..ad645b1 --- /dev/null +++ b/my_actions/collections/test_collections.yaml @@ -0,0 +1,8 @@ +collections: + - collection: + id: "1" + name: collection_1 + actions: + - "1" + - "2" + is_sequence: True diff --git a/my_actions/motion_scripts/sitdown.yaml b/my_actions/motion_scripts/sitdown.yaml new file mode 100644 index 0000000..27a66a2 --- /dev/null +++ b/my_actions/motion_scripts/sitdown.yaml @@ -0,0 +1,5 @@ +steps: + - step: + - base_auto: + height: 0.1 + ignore_timing_of_leg_motion: false diff --git a/my_actions/motion_scripts/standup.yaml b/my_actions/motion_scripts/standup.yaml new file mode 100644 index 0000000..dad3837 --- /dev/null +++ b/my_actions/motion_scripts/standup.yaml @@ -0,0 +1,5 @@ +steps: + - step: + - base_auto: + height: 0.4 + ignore_timing_of_leg_motion: false diff --git a/my_actions/package.xml b/my_actions/package.xml new file mode 100644 index 0000000..a7a94e7 --- /dev/null +++ b/my_actions/package.xml @@ -0,0 +1,69 @@ + + + my_actions + 0.0.0 + The my_actions package + + + + + hitstar + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + free_gait_action_loader + roscpp + rospy + free_gait_action_loader + roscpp + rospy + free_gait_action_loader + roscpp + rospy + + + + + + + + + diff --git a/quadruped_model/include/quadruped_model/quadruped_state.h b/quadruped_model/include/quadruped_model/quadruped_state.h index c21dd11..c4468ce 100644 --- a/quadruped_model/include/quadruped_model/quadruped_state.h +++ b/quadruped_model/include/quadruped_model/quadruped_state.h @@ -11,10 +11,13 @@ class QuadrupedState : public QuadrupedKinematics typedef std::unordered_map limb_joints; QuadrupedState(); ~QuadrupedState(); +// QuadrupedState(const QuadrupedState& other); + bool Initialize(); const Position getPositionWorldToBaseInWorldFrame() const; const RotationQuaternion getOrientationBaseToWorld() const; static JointPositions& getJointPositions(); static JointVelocities& getJointVelocities(); + const JointPositions& getJointPositionsToReach() const; const LinearVelocity getLinearVelocityBaseInWorldFrame() const; const LocalAngularVelocity getAngularVelocityBaseInBaseFrame() const; @@ -53,6 +56,7 @@ class QuadrupedState : public QuadrupedKinematics FootPoseInBase footPoseInBaseFrame_; Pose poseInWorldFrame_; static JointPositions joint_positions_, joint_positions_feedback_; + JointPositions allJointPositionsToReach_; static JointVelocities joint_velocities_; Position positionWorldToBaseInWorldFrame_; RotationQuaternion orientationBaseToWorld_; diff --git a/quadruped_model/src/quadruped_state.cpp b/quadruped_model/src/quadruped_state.cpp index b1e133f..ba31f4d 100644 --- a/quadruped_model/src/quadruped_state.cpp +++ b/quadruped_model/src/quadruped_state.cpp @@ -14,15 +14,31 @@ QuadrupedState::QuadrupedState() // LoadRobotDescriptionFromFile("/home/hitstar/catkin_ws/src/quadruped_locomotion-dev/quadruped_model/urdf/simpledog.urdf"); std::cout<<"Constructor QuadrupedState"< - +