From 40afb9ca1387243129b3a6cc781f84f65937c0cb Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 24 May 2019 20:55:36 +0900 Subject: [PATCH 01/27] add landing position port --- hrpsys_ros_bridge/CMakeLists.txt | 2 +- .../launch/hrpsys_ros_bridge.launch | 2 ++ hrpsys_ros_bridge/msg/LandingPosition.msg | 6 ++++ .../src/HrpsysSeqStateROSBridge.cpp | 32 +++++++++++++++++++ .../src/HrpsysSeqStateROSBridge.h | 6 ++-- .../src/HrpsysSeqStateROSBridgeImpl.cpp | 4 +++ .../src/HrpsysSeqStateROSBridgeImpl.h | 4 +++ 7 files changed, 53 insertions(+), 3 deletions(-) create mode 100644 hrpsys_ros_bridge/msg/LandingPosition.msg diff --git a/hrpsys_ros_bridge/CMakeLists.txt b/hrpsys_ros_bridge/CMakeLists.txt index 15594390e..0c2183ca3 100644 --- a/hrpsys_ros_bridge/CMakeLists.txt +++ b/hrpsys_ros_bridge/CMakeLists.txt @@ -76,7 +76,7 @@ if(CCACHE_FOUND AND "$ENV{CI}" STREQUAL "true" ) endif(CCACHE_FOUND AND "$ENV{CI}" STREQUAL "true" ) # define add_message_files before rtmbuild_init -add_message_files(FILES MotorStates.msg ContactState.msg ContactStateStamped.msg ContactStatesStamped.msg) +add_message_files(FILES MotorStates.msg LandingPosition.msg ContactState.msg ContactStateStamped.msg ContactStatesStamped.msg) add_service_files(FILES SetSensorTransformation.srv) # initialize rtmbuild rtmbuild_init(geometry_msgs) diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch index 6963b96ac..347e86b75 100644 --- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch +++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch @@ -169,6 +169,8 @@ + + diff --git a/hrpsys_ros_bridge/msg/LandingPosition.msg b/hrpsys_ros_bridge/msg/LandingPosition.msg new file mode 100644 index 000000000..363c0b306 --- /dev/null +++ b/hrpsys_ros_bridge/msg/LandingPosition.msg @@ -0,0 +1,6 @@ +Header header + +float64 x +float64 y +float64 z +int8 l_r diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index bebd13460..dacebaa18 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -62,6 +62,7 @@ HrpsysSeqStateROSBridge::HrpsysSeqStateROSBridge(RTC::Manager* manager) : joint_controller_state_pub = nh.advertise("/fullbody_controller/state", 1); #endif trajectory_command_sub = nh.subscribe("/fullbody_controller/command", 1, &HrpsysSeqStateROSBridge::onTrajectoryCommandCB, this); + landing_height_sub = nh.subscribe("/landing_height", 1, &HrpsysSeqStateROSBridge::onLandingHeightCB, this); mot_states_pub = nh.advertise("/motor_states", 1); odom_pub = nh.advertise("/odom", 1); imu_pub = nh.advertise("/imu", 1); @@ -158,6 +159,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onInitialize() { ref_contact_states_pub = nh.advertise("/ref_contact_states", 10); act_contact_states_pub = nh.advertise("/act_contact_states", 10); cop_pub.resize(m_mcforceName.size()); + landing_target_pub = nh.advertise("/landing_target", 10); for (unsigned int i=0; ix; + m_rslandingHeight.data.y = msg->y; + m_rslandingHeight.data.z = msg->z; + m_rslandingHeight.data.l_r = msg->l_r; + m_rslandingHeightOut.write(); +} + bool HrpsysSeqStateROSBridge::setSensorTransformation(hrpsys_ros_bridge::SetSensorTransformation::Request& req, hrpsys_ros_bridge::SetSensorTransformation::Response& res) { @@ -797,6 +807,28 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id) } } + if ( m_rslandingTargetIn.isNew() ) { + try { + m_rslandingTargetIn.read(); + hrpsys_ros_bridge::LandingPosition landingTargetv; + if ( use_hrpsys_time ) { + landingTargetv.header.stamp = ros::Time(m_rslandingTarget.tm.sec, m_rslandingTarget.tm.nsec); + }else{ + landingTargetv.header.stamp = tm_on_execute; + } + landingTargetv.header.frame_id = rootlink_name; + landingTargetv.x = m_rslandingTarget.data.x; + landingTargetv.y = m_rslandingTarget.data.y; + landingTargetv.z = m_rslandingTarget.data.z; + landingTargetv.l_r = m_rslandingTarget.data.l_r; + landing_target_pub.publish(landingTargetv); + } + catch(const std::runtime_error &e) + { + ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what()); + } + } + if ( m_refContactStatesIn.isNew() && m_controlSwingSupportTimeIn.isNew() ) { try { m_refContactStatesIn.read(); diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h index 4068ca1a3..191d1a2a5 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h @@ -30,6 +30,7 @@ #include "dynamic_reconfigure/Reconfigure.h" #include "hrpsys_ros_bridge/MotorStates.h" #include "hrpsys_ros_bridge/ContactStatesStamped.h" +#include "hrpsys_ros_bridge/LandingPosition.h" #include "diagnostic_msgs/DiagnosticArray.h" #include "sensor_msgs/Imu.h" #include "hrpsys_ros_bridge/SetSensorTransformation.h" @@ -54,14 +55,15 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl void onFollowJointTrajectoryActionGoal(); void onFollowJointTrajectoryActionPreempt(); void onTrajectoryCommandCB(const trajectory_msgs::JointTrajectoryConstPtr& msg); + void onLandingHeightCB(const hrpsys_ros_bridge::LandingPosition::ConstPtr& msg); bool sendMsg (dynamic_reconfigure::Reconfigure::Request &req, dynamic_reconfigure::Reconfigure::Response &res); bool setSensorTransformation(hrpsys_ros_bridge::SetSensorTransformation::Request& req, hrpsys_ros_bridge::SetSensorTransformation::Response& res); private: ros::NodeHandle nh; - ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub; - ros::Subscriber trajectory_command_sub; + ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub, landing_target_pub; + ros::Subscriber trajectory_command_sub, landing_height_sub; std::vector fsensor_pub, cop_pub; #ifdef USE_PR2_CONTROLLERS_MSGS actionlib::SimpleActionServer joint_trajectory_server; diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp index ac2a92496..7d0e7547f 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp @@ -45,6 +45,8 @@ HrpsysSeqStateROSBridgeImpl::HrpsysSeqStateROSBridgeImpl(RTC::Manager* manager) m_actContactStatesIn("actContactStates", m_actContactStates), m_controlSwingSupportTimeIn("controlSwingSupportTime", m_controlSwingSupportTime), m_mctorqueOut("mctorque", m_mctorque), + m_rslandingTargetIn("rslandingTarget", m_rslandingTarget), + m_rslandingHeightOut("rslandingHeight", m_rslandingHeight), m_SequencePlayerServicePort("SequencePlayerService") // @@ -76,9 +78,11 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onInitialize() addInPort("refContactStates", m_refContactStatesIn); addInPort("actContactStates", m_actContactStatesIn); addInPort("controlSwingSupportTime", m_controlSwingSupportTimeIn); + addInPort("rslandingTarget", m_rslandingTargetIn); // Set OutPort buffer addOutPort("mctorque", m_mctorqueOut); + addOutPort("rslandingHeight", m_rslandingHeightOut); // Set service provider to Ports diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h index 20c5dfb82..792d9649d 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h @@ -157,6 +157,8 @@ class HrpsysSeqStateROSBridgeImpl : public RTC::DataFlowComponentBase InPort m_refContactStatesIn, m_actContactStatesIn; TimedDoubleSeq m_controlSwingSupportTime; InPort m_controlSwingSupportTimeIn; + OpenHRP::TimedLandingPosition m_rslandingTarget; + InPort m_rslandingTargetIn; // @@ -164,6 +166,8 @@ class HrpsysSeqStateROSBridgeImpl : public RTC::DataFlowComponentBase // TimedDoubleSeq m_mctorque; OutPort m_mctorqueOut; + OpenHRP::TimedLandingPosition m_rslandingHeight; + OutPort m_rslandingHeightOut; // From 1a201714f13e7fbeaa904817b0390d69fd0ac07c Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 31 May 2019 14:35:36 +0900 Subject: [PATCH 02/27] publish next footstep rot --- hrpsys_ros_bridge/msg/LandingPosition.msg | 3 +++ hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp | 3 +++ 2 files changed, 6 insertions(+) diff --git a/hrpsys_ros_bridge/msg/LandingPosition.msg b/hrpsys_ros_bridge/msg/LandingPosition.msg index 363c0b306..61ec83856 100644 --- a/hrpsys_ros_bridge/msg/LandingPosition.msg +++ b/hrpsys_ros_bridge/msg/LandingPosition.msg @@ -3,4 +3,7 @@ Header header float64 x float64 y float64 z +float64 nx +float64 ny +float64 nz int8 l_r diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index dacebaa18..7ea96523f 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -281,6 +281,9 @@ void HrpsysSeqStateROSBridge::onLandingHeightCB(const hrpsys_ros_bridge::Landing m_rslandingHeight.data.x = msg->x; m_rslandingHeight.data.y = msg->y; m_rslandingHeight.data.z = msg->z; + m_rslandingHeight.data.nx = msg->nx; + m_rslandingHeight.data.ny = msg->ny; + m_rslandingHeight.data.nz = msg->nz; m_rslandingHeight.data.l_r = msg->l_r; m_rslandingHeightOut.write(); } From 64f94dc2c4d4a2567e7829b5ccbe547f1d2b7333 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Wed, 5 Jun 2019 12:48:29 +0900 Subject: [PATCH 03/27] publish steppable region --- hrpsys_ros_bridge/CMakeLists.txt | 2 +- .../launch/hrpsys_ros_bridge.launch | 1 + hrpsys_ros_bridge/msg/SteppableRegion.msg | 5 +++++ .../src/HrpsysSeqStateROSBridge.cpp | 16 ++++++++++++++++ hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h | 4 +++- .../src/HrpsysSeqStateROSBridgeImpl.cpp | 2 ++ .../src/HrpsysSeqStateROSBridgeImpl.h | 2 ++ 7 files changed, 30 insertions(+), 2 deletions(-) create mode 100644 hrpsys_ros_bridge/msg/SteppableRegion.msg diff --git a/hrpsys_ros_bridge/CMakeLists.txt b/hrpsys_ros_bridge/CMakeLists.txt index 0c2183ca3..011784778 100644 --- a/hrpsys_ros_bridge/CMakeLists.txt +++ b/hrpsys_ros_bridge/CMakeLists.txt @@ -76,7 +76,7 @@ if(CCACHE_FOUND AND "$ENV{CI}" STREQUAL "true" ) endif(CCACHE_FOUND AND "$ENV{CI}" STREQUAL "true" ) # define add_message_files before rtmbuild_init -add_message_files(FILES MotorStates.msg LandingPosition.msg ContactState.msg ContactStateStamped.msg ContactStatesStamped.msg) +add_message_files(FILES MotorStates.msg LandingPosition.msg SteppableRegion.msg ContactState.msg ContactStateStamped.msg ContactStatesStamped.msg) add_service_files(FILES SetSensorTransformation.srv) # initialize rtmbuild rtmbuild_init(geometry_msgs) diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch index 347e86b75..9ec39a2d0 100644 --- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch +++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch @@ -171,6 +171,7 @@ + diff --git a/hrpsys_ros_bridge/msg/SteppableRegion.msg b/hrpsys_ros_bridge/msg/SteppableRegion.msg new file mode 100644 index 000000000..862cd4ec2 --- /dev/null +++ b/hrpsys_ros_bridge/msg/SteppableRegion.msg @@ -0,0 +1,5 @@ +Header header +geometry_msgs/PolygonStamped[] polygons +uint32[] labels +float32[] likelihood +uint32 l_r \ No newline at end of file diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index 7ea96523f..5e41897bc 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -63,6 +63,7 @@ HrpsysSeqStateROSBridge::HrpsysSeqStateROSBridge(RTC::Manager* manager) : #endif trajectory_command_sub = nh.subscribe("/fullbody_controller/command", 1, &HrpsysSeqStateROSBridge::onTrajectoryCommandCB, this); landing_height_sub = nh.subscribe("/landing_height", 1, &HrpsysSeqStateROSBridge::onLandingHeightCB, this); + steppable_region_sub = nh.subscribe("/steppable_region", 1, &HrpsysSeqStateROSBridge::onSteppableRegionCB, this); mot_states_pub = nh.advertise("/motor_states", 1); odom_pub = nh.advertise("/odom", 1); imu_pub = nh.advertise("/imu", 1); @@ -288,6 +289,21 @@ void HrpsysSeqStateROSBridge::onLandingHeightCB(const hrpsys_ros_bridge::Landing m_rslandingHeightOut.write(); } +void HrpsysSeqStateROSBridge::onSteppableRegionCB(const hrpsys_ros_bridge::SteppableRegion::ConstPtr& msg) { + size_t convex_num(msg->polygons.size()); + m_rssteppableRegion.data.region.length(convex_num); + for (size_t i = 0; i < convex_num; i++) { + size_t vs_num(msg->polygons[i].polygon.points.size()); + m_rssteppableRegion.data.region[i].length(2 * vs_num); // x,y components + for (size_t j = 0; j < vs_num; j++) { + m_rssteppableRegion.data.region[i][2*j] = msg->polygons[i].polygon.points[j].x; + m_rssteppableRegion.data.region[i][2*j+1] = msg->polygons[i].polygon.points[j].y; + } + } + m_rssteppableRegion.data.l_r = msg->l_r; + m_rssteppableRegionOut.write(); +} + bool HrpsysSeqStateROSBridge::setSensorTransformation(hrpsys_ros_bridge::SetSensorTransformation::Request& req, hrpsys_ros_bridge::SetSensorTransformation::Response& res) { diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h index 191d1a2a5..bb56c023a 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h @@ -31,6 +31,7 @@ #include "hrpsys_ros_bridge/MotorStates.h" #include "hrpsys_ros_bridge/ContactStatesStamped.h" #include "hrpsys_ros_bridge/LandingPosition.h" +#include "hrpsys_ros_bridge/SteppableRegion.h" #include "diagnostic_msgs/DiagnosticArray.h" #include "sensor_msgs/Imu.h" #include "hrpsys_ros_bridge/SetSensorTransformation.h" @@ -56,6 +57,7 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl void onFollowJointTrajectoryActionPreempt(); void onTrajectoryCommandCB(const trajectory_msgs::JointTrajectoryConstPtr& msg); void onLandingHeightCB(const hrpsys_ros_bridge::LandingPosition::ConstPtr& msg); + void onSteppableRegionCB(const hrpsys_ros_bridge::SteppableRegion::ConstPtr& msg); bool sendMsg (dynamic_reconfigure::Reconfigure::Request &req, dynamic_reconfigure::Reconfigure::Response &res); bool setSensorTransformation(hrpsys_ros_bridge::SetSensorTransformation::Request& req, @@ -63,7 +65,7 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl private: ros::NodeHandle nh; ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub, landing_target_pub; - ros::Subscriber trajectory_command_sub, landing_height_sub; + ros::Subscriber trajectory_command_sub, landing_height_sub, steppable_region_sub; std::vector fsensor_pub, cop_pub; #ifdef USE_PR2_CONTROLLERS_MSGS actionlib::SimpleActionServer joint_trajectory_server; diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp index 7d0e7547f..d08b2e32e 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp @@ -47,6 +47,7 @@ HrpsysSeqStateROSBridgeImpl::HrpsysSeqStateROSBridgeImpl(RTC::Manager* manager) m_mctorqueOut("mctorque", m_mctorque), m_rslandingTargetIn("rslandingTarget", m_rslandingTarget), m_rslandingHeightOut("rslandingHeight", m_rslandingHeight), + m_rssteppableRegionOut("rssteppableRegion", m_rssteppableRegion), m_SequencePlayerServicePort("SequencePlayerService") // @@ -83,6 +84,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onInitialize() // Set OutPort buffer addOutPort("mctorque", m_mctorqueOut); addOutPort("rslandingHeight", m_rslandingHeightOut); + addOutPort("rssteppableRegion", m_rssteppableRegionOut); // Set service provider to Ports diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h index 792d9649d..6e1bc1188 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h @@ -168,6 +168,8 @@ class HrpsysSeqStateROSBridgeImpl : public RTC::DataFlowComponentBase OutPort m_mctorqueOut; OpenHRP::TimedLandingPosition m_rslandingHeight; OutPort m_rslandingHeightOut; + OpenHRP::TimedSteppableRegion m_rssteppableRegion; + OutPort m_rssteppableRegionOut; // From 7bff1a5e66620fec63314ef63b45ac4c34057ae9 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 23 Aug 2019 18:20:53 +0900 Subject: [PATCH 04/27] publish end_cog_state --- hrpsys_ros_bridge/CMakeLists.txt | 2 +- .../launch/hrpsys_ros_bridge.launch | 1 + hrpsys_ros_bridge/msg/CogState.msg | 9 +++++++ .../src/HrpsysSeqStateROSBridge.cpp | 26 +++++++++++++++++++ .../src/HrpsysSeqStateROSBridge.h | 3 ++- .../src/HrpsysSeqStateROSBridgeImpl.cpp | 2 ++ .../src/HrpsysSeqStateROSBridgeImpl.h | 2 ++ 7 files changed, 43 insertions(+), 2 deletions(-) create mode 100644 hrpsys_ros_bridge/msg/CogState.msg diff --git a/hrpsys_ros_bridge/CMakeLists.txt b/hrpsys_ros_bridge/CMakeLists.txt index 011784778..5960b3ab1 100644 --- a/hrpsys_ros_bridge/CMakeLists.txt +++ b/hrpsys_ros_bridge/CMakeLists.txt @@ -76,7 +76,7 @@ if(CCACHE_FOUND AND "$ENV{CI}" STREQUAL "true" ) endif(CCACHE_FOUND AND "$ENV{CI}" STREQUAL "true" ) # define add_message_files before rtmbuild_init -add_message_files(FILES MotorStates.msg LandingPosition.msg SteppableRegion.msg ContactState.msg ContactStateStamped.msg ContactStatesStamped.msg) +add_message_files(FILES MotorStates.msg LandingPosition.msg SteppableRegion.msg CogState.msg ContactState.msg ContactStateStamped.msg ContactStatesStamped.msg) add_service_files(FILES SetSensorTransformation.srv) # initialize rtmbuild rtmbuild_init(geometry_msgs) diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch index 9ec39a2d0..ebc962d01 100644 --- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch +++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch @@ -170,6 +170,7 @@ + diff --git a/hrpsys_ros_bridge/msg/CogState.msg b/hrpsys_ros_bridge/msg/CogState.msg new file mode 100644 index 000000000..f47603f41 --- /dev/null +++ b/hrpsys_ros_bridge/msg/CogState.msg @@ -0,0 +1,9 @@ +Header header + +float64 x +float64 y +float64 z +float64 vx +float64 vy +float64 vz +int8 l_r diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index 5e41897bc..529684f7f 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -161,6 +161,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onInitialize() { act_contact_states_pub = nh.advertise("/act_contact_states", 10); cop_pub.resize(m_mcforceName.size()); landing_target_pub = nh.advertise("/landing_target", 10); + end_cog_state_pub = nh.advertise("/end_cog_state", 10); for (unsigned int i=0; i fsensor_pub, cop_pub; #ifdef USE_PR2_CONTROLLERS_MSGS diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp index d08b2e32e..e5bf75215 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp @@ -46,6 +46,7 @@ HrpsysSeqStateROSBridgeImpl::HrpsysSeqStateROSBridgeImpl(RTC::Manager* manager) m_controlSwingSupportTimeIn("controlSwingSupportTime", m_controlSwingSupportTime), m_mctorqueOut("mctorque", m_mctorque), m_rslandingTargetIn("rslandingTarget", m_rslandingTarget), + m_rsendCogStateIn("rsendCogState", m_rsendCogState), m_rslandingHeightOut("rslandingHeight", m_rslandingHeight), m_rssteppableRegionOut("rssteppableRegion", m_rssteppableRegion), m_SequencePlayerServicePort("SequencePlayerService") @@ -80,6 +81,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onInitialize() addInPort("actContactStates", m_actContactStatesIn); addInPort("controlSwingSupportTime", m_controlSwingSupportTimeIn); addInPort("rslandingTarget", m_rslandingTargetIn); + addInPort("rsendCogState", m_rsendCogStateIn); // Set OutPort buffer addOutPort("mctorque", m_mctorqueOut); diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h index 6e1bc1188..5e9778380 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h @@ -159,6 +159,8 @@ class HrpsysSeqStateROSBridgeImpl : public RTC::DataFlowComponentBase InPort m_controlSwingSupportTimeIn; OpenHRP::TimedLandingPosition m_rslandingTarget; InPort m_rslandingTargetIn; + OpenHRP::TimedCogState m_rsendCogState; + InPort m_rsendCogStateIn; // From aa1b144af4512887efa04e427f3ea6eef2af0638 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 13 Dec 2019 15:59:23 +0900 Subject: [PATCH 05/27] publish is_stuck state --- .../euslisp/rtm-ros-robot-interface.l | 7 +++++++ hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch | 1 + hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp | 14 ++++++++++++++ hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h | 2 +- .../src/HrpsysSeqStateROSBridgeImpl.cpp | 2 ++ .../src/HrpsysSeqStateROSBridgeImpl.h | 2 ++ 6 files changed, 27 insertions(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index da2cf0d79..cc8a83daf 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -32,6 +32,8 @@ #'send self :rtmros-imu-callback :groupname groupname) (ros::subscribe "/emergency_mode" std_msgs::Int32 #'send self :rtmros-emergency-mode-callback :groupname groupname) + (ros::subscribe "/is_stuck" std_msgs::Int32 + #'send self :rtmros-is-stuck-callback :groupname groupname) (mapcar #'(lambda (x) (ros::subscribe (format nil "/~A_capture_point" x) geometry_msgs::PointStamped #'send self :rtmros-capture-point-callback (read-from-string (format nil ":~A-capture-point" x)) :groupname groupname)) @@ -69,6 +71,11 @@ (let ((m (send msg :data))) (send self :set-robot-state1 :emergency-mode m)) ) + (:rtmros-is-stuck-callback + (msg) + (let ((m (send msg :data))) + (send self :set-robot-state1 :is-stuck m)) + ) (:rtmros-capture-point-callback (ref-act msg) (let ((p (send msg :point))) diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch index ebc962d01..610a80cbf 100644 --- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch +++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch @@ -264,6 +264,7 @@ + diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index 529684f7f..0e01cb7da 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -168,6 +168,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onInitialize() { cop_pub[i] = nh.advertise(tmpname+"_cop", 10); } em_mode_pub = nh.advertise("emergency_mode", 10); + is_stuck_pub = nh.advertise("is_stuck", 10); return RTC::RTC_OK; } @@ -979,6 +980,19 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id) } } + if ( m_isStuckIn.isNew() ) { + try { + m_isStuckIn.read(); + std_msgs::Int32 is_stuck; + is_stuck.data = m_isStuck.data; + is_stuck_pub.publish(is_stuck); + } + catch(const std::runtime_error &e) + { + ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what()); + } + } + // return RTC::RTC_OK; } diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h index bf91b90ac..40816f0ed 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h @@ -65,7 +65,7 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl hrpsys_ros_bridge::SetSensorTransformation::Response& res); private: ros::NodeHandle nh; - ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub, landing_target_pub, end_cog_state_pub; + ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub, landing_target_pub, end_cog_state_pub, is_stuck_pub; ros::Subscriber trajectory_command_sub, landing_height_sub, steppable_region_sub; std::vector fsensor_pub, cop_pub; #ifdef USE_PR2_CONTROLLERS_MSGS diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp index e5bf75215..85e02f73e 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp @@ -41,6 +41,7 @@ HrpsysSeqStateROSBridgeImpl::HrpsysSeqStateROSBridgeImpl(RTC::Manager* manager) m_rsactCPIn("rsactCapturePoint", m_rsactCP), m_rsCOPInfoIn("rsCOPInfo", m_rsCOPInfo), m_emergencyModeIn("emergencyMode", m_emergencyMode), + m_isStuckIn("isStuck", m_isStuck), m_refContactStatesIn("refContactStates", m_refContactStates), m_actContactStatesIn("actContactStates", m_actContactStates), m_controlSwingSupportTimeIn("controlSwingSupportTime", m_controlSwingSupportTime), @@ -77,6 +78,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onInitialize() addInPort("servoState", m_servoStateIn); addInPort("rsCOPInfo", m_rsCOPInfoIn); addInPort("emergencyMode", m_emergencyModeIn); + addInPort("isStuck", m_isStuckIn); addInPort("refContactStates", m_refContactStatesIn); addInPort("actContactStates", m_actContactStatesIn); addInPort("controlSwingSupportTime", m_controlSwingSupportTimeIn); diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h index 5e9778380..863dba89d 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h @@ -153,6 +153,8 @@ class HrpsysSeqStateROSBridgeImpl : public RTC::DataFlowComponentBase InPort m_rsCOPInfoIn; TimedLong m_emergencyMode; InPort m_emergencyModeIn; + TimedBoolean m_isStuck; + InPort m_isStuckIn; TimedBooleanSeq m_refContactStates, m_actContactStates; InPort m_refContactStatesIn, m_actContactStatesIn; TimedDoubleSeq m_controlSwingSupportTime; From 54dfbbeaf2124ac7b4c1a90389fa6cc4239fe601 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 13 Dec 2019 19:28:44 +0900 Subject: [PATCH 06/27] publish estimated fxy --- .../euslisp/rtm-ros-robot-interface.l | 6 +++++ .../launch/hrpsys_ros_bridge.launch | 1 + .../src/HrpsysSeqStateROSBridge.cpp | 23 +++++++++++++++++++ .../src/HrpsysSeqStateROSBridge.h | 2 +- .../src/HrpsysSeqStateROSBridgeImpl.cpp | 2 ++ .../src/HrpsysSeqStateROSBridgeImpl.h | 2 ++ 6 files changed, 35 insertions(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index cc8a83daf..93ccb5909 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -34,6 +34,8 @@ #'send self :rtmros-emergency-mode-callback :groupname groupname) (ros::subscribe "/is_stuck" std_msgs::Int32 #'send self :rtmros-is-stuck-callback :groupname groupname) + (ros::subscribe "/estimated_fxy" geometry_msgs::PointStamped + #'send self :rtmros-estimated-fxy-callback :groupname groupname) (mapcar #'(lambda (x) (ros::subscribe (format nil "/~A_capture_point" x) geometry_msgs::PointStamped #'send self :rtmros-capture-point-callback (read-from-string (format nil ":~A-capture-point" x)) :groupname groupname)) @@ -76,6 +78,10 @@ (let ((m (send msg :data))) (send self :set-robot-state1 :is-stuck m)) ) + (:rtmros-estimated-fxy-callback + (msg) + (let ((p (send msg :point))) + (send self :set-robot-state1 :estimated-fxy (float-vector (send p :x) (send p :y) (send p :z))))) (:rtmros-capture-point-callback (ref-act msg) (let ((p (send msg :point))) diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch index 610a80cbf..07ab6946d 100644 --- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch +++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch @@ -265,6 +265,7 @@ output="screen" args ="$(arg openrtm_args)" /> + diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index 0e01cb7da..65f9c6927 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -169,6 +169,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onInitialize() { } em_mode_pub = nh.advertise("emergency_mode", 10); is_stuck_pub = nh.advertise("is_stuck", 10); + estimated_fxy_pub = nh.advertise("estimated_fxy", 10); return RTC::RTC_OK; } @@ -993,6 +994,28 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id) } } + if ( m_estimatedFxyIn.isNew() ) { + try { + m_estimatedFxyIn.read(); + geometry_msgs::PointStamped fxyv; + if ( use_hrpsys_time ) { + fxyv.header.stamp = ros::Time(m_estimatedFxy.tm.sec, m_estimatedFxy.tm.nsec); + }else{ + fxyv.header.stamp = tm_on_execute; + } + fxyv.header.frame_id = rootlink_name; + fxyv.point.x = m_estimatedFxy.data.x; + fxyv.point.y = m_estimatedFxy.data.y; + fxyv.point.z = m_estimatedFxy.data.z; + estimated_fxy_pub.publish(fxyv); + } + catch(const std::runtime_error &e) + { + ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what()); + } + } + + // return RTC::RTC_OK; } diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h index 40816f0ed..4f951daac 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h @@ -65,7 +65,7 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl hrpsys_ros_bridge::SetSensorTransformation::Response& res); private: ros::NodeHandle nh; - ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub, landing_target_pub, end_cog_state_pub, is_stuck_pub; + ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub, landing_target_pub, end_cog_state_pub, is_stuck_pub, estimated_fxy_pub; ros::Subscriber trajectory_command_sub, landing_height_sub, steppable_region_sub; std::vector fsensor_pub, cop_pub; #ifdef USE_PR2_CONTROLLERS_MSGS diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp index 85e02f73e..25d86888b 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp @@ -42,6 +42,7 @@ HrpsysSeqStateROSBridgeImpl::HrpsysSeqStateROSBridgeImpl(RTC::Manager* manager) m_rsCOPInfoIn("rsCOPInfo", m_rsCOPInfo), m_emergencyModeIn("emergencyMode", m_emergencyMode), m_isStuckIn("isStuck", m_isStuck), + m_estimatedFxyIn("estimatedFxy", m_estimatedFxy), m_refContactStatesIn("refContactStates", m_refContactStates), m_actContactStatesIn("actContactStates", m_actContactStates), m_controlSwingSupportTimeIn("controlSwingSupportTime", m_controlSwingSupportTime), @@ -79,6 +80,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onInitialize() addInPort("rsCOPInfo", m_rsCOPInfoIn); addInPort("emergencyMode", m_emergencyModeIn); addInPort("isStuck", m_isStuckIn); + addInPort("estimatedFxy", m_estimatedFxyIn); addInPort("refContactStates", m_refContactStatesIn); addInPort("actContactStates", m_actContactStatesIn); addInPort("controlSwingSupportTime", m_controlSwingSupportTimeIn); diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h index 863dba89d..1b31bdfaf 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h @@ -155,6 +155,8 @@ class HrpsysSeqStateROSBridgeImpl : public RTC::DataFlowComponentBase InPort m_emergencyModeIn; TimedBoolean m_isStuck; InPort m_isStuckIn; + TimedPoint3D m_estimatedFxy; + InPort m_estimatedFxyIn; TimedBooleanSeq m_refContactStates, m_actContactStates; InPort m_refContactStatesIn, m_actContactStatesIn; TimedDoubleSeq m_controlSwingSupportTime; From dfb0453310ce6b0a85fbe4bd411ad64740509508 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Thu, 9 Jan 2020 18:29:18 +0900 Subject: [PATCH 07/27] publish /use_flywheel --- .../euslisp/rtm-ros-robot-interface.l | 7 +++++++ hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch | 1 + hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp | 14 ++++++++++++++ hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h | 2 +- .../src/HrpsysSeqStateROSBridgeImpl.cpp | 2 ++ .../src/HrpsysSeqStateROSBridgeImpl.h | 2 ++ 6 files changed, 27 insertions(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index 93ccb5909..234477a74 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -34,6 +34,8 @@ #'send self :rtmros-emergency-mode-callback :groupname groupname) (ros::subscribe "/is_stuck" std_msgs::Int32 #'send self :rtmros-is-stuck-callback :groupname groupname) + (ros::subscribe "/use_flywheel" std_msgs::Int32 + #'send self :rtmros-use-flywheel-callback :groupname groupname) (ros::subscribe "/estimated_fxy" geometry_msgs::PointStamped #'send self :rtmros-estimated-fxy-callback :groupname groupname) (mapcar #'(lambda (x) @@ -78,6 +80,11 @@ (let ((m (send msg :data))) (send self :set-robot-state1 :is-stuck m)) ) + (:rtmros-use-flywheel-callback + (msg) + (let ((m (send msg :data))) + (send self :set-robot-state1 :use-flywheel m)) + ) (:rtmros-estimated-fxy-callback (msg) (let ((p (send msg :point))) diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch index 07ab6946d..3a542280f 100644 --- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch +++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch @@ -265,6 +265,7 @@ output="screen" args ="$(arg openrtm_args)" /> + diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index 65f9c6927..4ec51bd80 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -169,6 +169,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onInitialize() { } em_mode_pub = nh.advertise("emergency_mode", 10); is_stuck_pub = nh.advertise("is_stuck", 10); + use_flywheel_pub = nh.advertise("use_flywheel", 10); estimated_fxy_pub = nh.advertise("estimated_fxy", 10); return RTC::RTC_OK; @@ -994,6 +995,19 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id) } } + if ( m_useFlywheelIn.isNew() ) { + try { + m_useFlywheelIn.read(); + std_msgs::Int32 use_flywheel; + use_flywheel.data = m_useFlywheel.data; + use_flywheel_pub.publish(use_flywheel); + } + catch(const std::runtime_error &e) + { + ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what()); + } + } + if ( m_estimatedFxyIn.isNew() ) { try { m_estimatedFxyIn.read(); diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h index 4f951daac..8bb5789f9 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h @@ -65,7 +65,7 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl hrpsys_ros_bridge::SetSensorTransformation::Response& res); private: ros::NodeHandle nh; - ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub, landing_target_pub, end_cog_state_pub, is_stuck_pub, estimated_fxy_pub; + ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub, landing_target_pub, end_cog_state_pub, is_stuck_pub, use_flywheel_pub, estimated_fxy_pub; ros::Subscriber trajectory_command_sub, landing_height_sub, steppable_region_sub; std::vector fsensor_pub, cop_pub; #ifdef USE_PR2_CONTROLLERS_MSGS diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp index 25d86888b..946eda852 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp @@ -42,6 +42,7 @@ HrpsysSeqStateROSBridgeImpl::HrpsysSeqStateROSBridgeImpl(RTC::Manager* manager) m_rsCOPInfoIn("rsCOPInfo", m_rsCOPInfo), m_emergencyModeIn("emergencyMode", m_emergencyMode), m_isStuckIn("isStuck", m_isStuck), + m_useFlywheelIn("useFlywheel", m_useFlywheel), m_estimatedFxyIn("estimatedFxy", m_estimatedFxy), m_refContactStatesIn("refContactStates", m_refContactStates), m_actContactStatesIn("actContactStates", m_actContactStates), @@ -80,6 +81,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onInitialize() addInPort("rsCOPInfo", m_rsCOPInfoIn); addInPort("emergencyMode", m_emergencyModeIn); addInPort("isStuck", m_isStuckIn); + addInPort("useFlywheel", m_useFlywheelIn); addInPort("estimatedFxy", m_estimatedFxyIn); addInPort("refContactStates", m_refContactStatesIn); addInPort("actContactStates", m_actContactStatesIn); diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h index 1b31bdfaf..29ed040f6 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h @@ -155,6 +155,8 @@ class HrpsysSeqStateROSBridgeImpl : public RTC::DataFlowComponentBase InPort m_emergencyModeIn; TimedBoolean m_isStuck; InPort m_isStuckIn; + TimedBoolean m_useFlywheel; + InPort m_useFlywheelIn; TimedPoint3D m_estimatedFxy; InPort m_estimatedFxyIn; TimedBooleanSeq m_refContactStates, m_actContactStates; From ca54b042ed86e5f43bec3dfab22fc1810654a1f0 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Tue, 2 Jun 2020 18:21:38 +0900 Subject: [PATCH 08/27] remove stabilizer service --- .../euslisp/rtm-ros-robot-interface.l | 14 +++++++------- hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch | 12 ++++++------ 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index 234477a74..f2c29f6f5 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -1774,7 +1774,7 @@ (def-set-get-param-method 'hrpsys_ros_bridge::Openhrp_StabilizerService_stParam :raw-set-st-param :get-st-param :get-st-param-arguments - :stabilizerservice_setparameter :stabilizerservice_getparameter) + :autobalancerservice_setstabilizerparam :autobalancerservice_getstabilizerparam) (defmethod rtm-ros-robot-interface (:set-st-param @@ -1850,7 +1850,7 @@ "Set Stabilzier parameter by default parameters." (unless (send self :get :default-st-param) (send self :put :default-st-param (send self :get-st-param))) - (send self :stabilizerservice_setparameter :i_param (send self :get :default-st-param)) + (send self :autobalancerservice_setparameter :i_param (send self :get :default-st-param)) ) (:set-st-param-by-ratio (state-feedback-gain-ratio @@ -1896,12 +1896,12 @@ (:start-st () "Start Stabilizer Mode." - (send self :stabilizerservice_startstabilizer) + (send self :autobalancerservice_startstabilizer) ) (:stop-st () "Stop Stabilizer Mode." - (send self :stabilizerservice_stopstabilizer) + (send self :autobalancerservice_stopstabilizer) ) ) @@ -2224,9 +2224,9 @@ (print-ros-msg (send self :get-gait-generator-param)) (format t ";; :get-auto-balancer-param~%") (print-ros-msg (send self :get-auto-balancer-param))) - (when (find "/StabilizerServiceROSBridge" rosbridge-nodes :test #'string=) - (format t ";; :get-st-param~%") - (print-ros-msg (send self :get-st-param))) + ;; (when (find "/StabilizerServiceROSBridge" rosbridge-nodes :test #'string=) + ;; (format t ";; :get-st-param~%") + ;; (print-ros-msg (send self :get-st-param))) (when (find "/ObjectContactTurnaroundDetectorServiceROSBridge" rosbridge-nodes :test #'string=) (format t ";; :get-object-turnaround-detector-param~%") (print-ros-msg (send self :get-object-turnaround-detector-param))) diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch index 3a542280f..b2f0f97b4 100644 --- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch +++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch @@ -161,14 +161,14 @@ - - - - + + + + - + - + From 63a8d78b6b1da73e48d1e25a7c1e869e791bc712 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Wed, 11 Nov 2020 18:02:17 +0900 Subject: [PATCH 09/27] fix bug; enable set st param from eus --- .travis | 2 +- hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis b/.travis index 74e968928..54cfece8b 160000 --- a/.travis +++ b/.travis @@ -1 +1 @@ -Subproject commit 74e968928601f613b2cec821c1a5975baafc1127 +Subproject commit 54cfece8bf0ab09877f00070a64a93fc507b68e7 diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index f2c29f6f5..0e6509254 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -1772,7 +1772,7 @@ ;; StabilizerService (def-set-get-param-method - 'hrpsys_ros_bridge::Openhrp_StabilizerService_stParam + 'hrpsys_ros_bridge::Openhrp_AutoBalancerService_StabilizerParam :raw-set-st-param :get-st-param :get-st-param-arguments :autobalancerservice_setstabilizerparam :autobalancerservice_getstabilizerparam) From 7ad0940bd651d8921e67bae4d62310999c15d37d Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Tue, 1 Dec 2020 18:03:05 +0900 Subject: [PATCH 10/27] [rtm-ros-robot-interface.l]fix bug in :cmd-vel-cb --- hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index 0e6509254..c04960c35 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -1667,7 +1667,7 @@ (send self :go-velocity (* vel-x-ratio (send (send msg :linear) :x)) (* vel-y-ratio (send (send msg :linear) :y)) - (* vel-th-ratio (send (send msg :angular) :z))) + (* vel-th-ratio (rad2deg (send (send msg :angular) :z)))) ) (:cmd-vel-mode () From c5691b97401e71911176cd5e9b83da4f4b8ba26b Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Tue, 1 Dec 2020 18:08:25 +0900 Subject: [PATCH 11/27] [rtm-ros-robot-interface.l]add comment to :go-velocity --- hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index c04960c35..1755baa57 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -1481,7 +1481,7 @@ )) (:go-velocity (vx vy vth) - "Call goVelocity." + "Call goVelocity. vx[m/s], vy[m/s], and vth[deg/s]" (send self :autobalancerservice_goVelocity :vx vx :vy vy :vth vth)) (:go-stop () From dbbf0c8f5d0c664356204891403410f5c21843ae Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Tue, 1 Dec 2020 18:43:19 +0900 Subject: [PATCH 12/27] [rtm-ros-robot-interface.l] fix bug in :cmd-vel-mode --- hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index 1755baa57..fc7def033 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -1673,8 +1673,8 @@ () "Walk with subscribing /cmd_vel topic." (send self :start-cmd-vel-mode) + (send self :go-velocity 0 0 0) (do-until-key - (send self :go-velocity 0 0 0) (ros::spin-once) (ros::sleep) ) From 1cebe4eb9c6aae55e8eb6c3a0b22af601bb58fb8 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Sat, 12 Dec 2020 01:08:08 +0900 Subject: [PATCH 13/27] publish current steppable region for debug --- .../launch/hrpsys_ros_bridge.launch | 1 + .../src/HrpsysSeqStateROSBridge.cpp | 28 +++++++++++++++++++ .../src/HrpsysSeqStateROSBridge.h | 2 +- .../src/HrpsysSeqStateROSBridgeImpl.cpp | 2 ++ .../src/HrpsysSeqStateROSBridgeImpl.h | 2 ++ 5 files changed, 34 insertions(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch index b2f0f97b4..2d2932b6e 100644 --- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch +++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch @@ -267,6 +267,7 @@ + diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index 4ec51bd80..e2245374d 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -171,6 +171,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onInitialize() { is_stuck_pub = nh.advertise("is_stuck", 10); use_flywheel_pub = nh.advertise("use_flywheel", 10); estimated_fxy_pub = nh.advertise("estimated_fxy", 10); + current_steppable_region_pub = nh.advertise("current_steppable_region", 10); return RTC::RTC_OK; } @@ -1029,6 +1030,33 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id) } } + if ( m_currentSteppableRegionIn.isNew() ) { + try { + m_currentSteppableRegionIn.read(); + hrpsys_ros_bridge::SteppableRegion region; + bool is_valid = false; + size_t convex_num(m_currentSteppableRegion.data.region.length()); + region.polygons.resize(convex_num); + for (size_t i = 0; i < convex_num; i++) { + size_t vs_num(m_currentSteppableRegion.data.region[i].length()/2); + if (vs_num > 3) is_valid = true; + region.polygons[i].polygon.points.resize(vs_num); + for (size_t j = 0; j < vs_num; j++) { + region.polygons[i].polygon.points[j].x = m_currentSteppableRegion.data.region[i][2*j]; + region.polygons[i].polygon.points[j].y = m_currentSteppableRegion.data.region[i][2*j+1]; + region.polygons[i].polygon.points[j].z = 0.0; + } + } + region.l_r = m_currentSteppableRegion.data.l_r; + if (is_valid) current_steppable_region_pub.publish(region); + } + catch(const std::runtime_error &e) + { + ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what()); + } + } + + // return RTC::RTC_OK; diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h index 8bb5789f9..a7a907312 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h @@ -65,7 +65,7 @@ class HrpsysSeqStateROSBridge : public HrpsysSeqStateROSBridgeImpl hrpsys_ros_bridge::SetSensorTransformation::Response& res); private: ros::NodeHandle nh; - ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub, landing_target_pub, end_cog_state_pub, is_stuck_pub, use_flywheel_pub, estimated_fxy_pub; + ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, ref_cp_pub, act_cp_pub, odom_pub, imu_pub, em_mode_pub, ref_contact_states_pub, act_contact_states_pub, landing_target_pub, end_cog_state_pub, is_stuck_pub, use_flywheel_pub, estimated_fxy_pub, current_steppable_region_pub; ros::Subscriber trajectory_command_sub, landing_height_sub, steppable_region_sub; std::vector fsensor_pub, cop_pub; #ifdef USE_PR2_CONTROLLERS_MSGS diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp index 946eda852..3b5fd4b56 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp @@ -52,6 +52,7 @@ HrpsysSeqStateROSBridgeImpl::HrpsysSeqStateROSBridgeImpl(RTC::Manager* manager) m_rsendCogStateIn("rsendCogState", m_rsendCogState), m_rslandingHeightOut("rslandingHeight", m_rslandingHeight), m_rssteppableRegionOut("rssteppableRegion", m_rssteppableRegion), + m_currentSteppableRegionIn("currentSteppableRegion", m_currentSteppableRegion), m_SequencePlayerServicePort("SequencePlayerService") // @@ -88,6 +89,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onInitialize() addInPort("controlSwingSupportTime", m_controlSwingSupportTimeIn); addInPort("rslandingTarget", m_rslandingTargetIn); addInPort("rsendCogState", m_rsendCogStateIn); + addInPort("currentSteppableRegion", m_currentSteppableRegionIn); // Set OutPort buffer addOutPort("mctorque", m_mctorqueOut); diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h index 29ed040f6..44472a201 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h @@ -167,6 +167,8 @@ class HrpsysSeqStateROSBridgeImpl : public RTC::DataFlowComponentBase InPort m_rslandingTargetIn; OpenHRP::TimedCogState m_rsendCogState; InPort m_rsendCogStateIn; + OpenHRP::TimedSteppableRegion m_currentSteppableRegion; + InPort m_currentSteppableRegionIn; // From f81550be8656401f3b34ffcbb84c8b9b4beef159 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Tue, 8 Jun 2021 17:34:58 +0900 Subject: [PATCH 14/27] support samplerobot --- .../test/samplerobot_hrpsys_config.py | 49 ++++++++++++++----- 1 file changed, 37 insertions(+), 12 deletions(-) diff --git a/hrpsys_ros_bridge/test/samplerobot_hrpsys_config.py b/hrpsys_ros_bridge/test/samplerobot_hrpsys_config.py index b10bbbcd2..1e06df221 100755 --- a/hrpsys_ros_bridge/test/samplerobot_hrpsys_config.py +++ b/hrpsys_ros_bridge/test/samplerobot_hrpsys_config.py @@ -30,25 +30,27 @@ def setStAbcParameters (self): tlp.debug_print_freq = int(10 / 0.002) tlp.alarmRatio = 1.0 self.tl_svc.setParameter(tlp) + abcp=self.abc_svc.getAutoBalancerParam()[1] + abcp.ik_mode = OpenHRP.AutoBalancerService.FULLBODY # ST parameters - stp=self.st_svc.getParameter() - stp.st_algorithm=OpenHRP.StabilizerService.EEFMQP + stp=self.abc_svc.getStabilizerParam() + stp.st_algorithm=OpenHRP.AutoBalancerService.EEFMQP # eefm st params tmp_leg_inside_margin=71.12*1e-3 tmp_leg_outside_margin=71.12*1e-3 tmp_leg_front_margin=182.0*1e-3 tmp_leg_rear_margin=72.0*1e-3 - rleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])] - lleg_vertices = [OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]), - OpenHRP.StabilizerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] + rleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_inside_margin])] + lleg_vertices = [OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, tmp_leg_outside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[tmp_leg_front_margin, -1*tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, -1*tmp_leg_inside_margin]), + OpenHRP.AutoBalancerService.TwoDimensionVertex(pos=[-1*tmp_leg_rear_margin, tmp_leg_outside_margin])] rarm_vertices = rleg_vertices larm_vertices = lleg_vertices - stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.StabilizerService.SupportPolygonVertices(vertices=x), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices]) + stp.eefm_support_polygon_vertices_sequence = map (lambda x : OpenHRP.AutoBalancerService.SupportPolygonVertices(vertices=x), [lleg_vertices, rleg_vertices, larm_vertices, rarm_vertices]) stp.eefm_leg_inside_margin=tmp_leg_inside_margin stp.eefm_leg_outside_margin=tmp_leg_outside_margin stp.eefm_leg_front_margin=tmp_leg_front_margin @@ -63,7 +65,30 @@ def setStAbcParameters (self): stp.k_tpcc_x=[4.0, 4.0] stp.k_brot_p=[0.0, 0.0] stp.k_brot_tc=[0.1, 0.1] - self.st_svc.setParameter(stp) + self.abc_svc.setStabilizerParam(stp) + gg=self.abc_svc.getGaitGeneratorParam()[1] + gg.default_step_time=1.0 + gg.default_step_height=0.05 + gg.default_double_support_ratio=0.15 + gg.swing_trajectory_delay_time_offset=0.15 + gg.stair_trajectory_way_point_offset=[0.03, 0.0, 0.0] + gg.swing_trajectory_final_distance_weight=3.0 + gg.leg_margin=[0.18, 0.07, 0.07, 0.07] + gg.safe_leg_margin=[0.15, 0.05, 0.05, 0.05] + gg.stride_limitation_for_circle_type=[0.15, 0.3, 15, 0.1, 0.13] + gg.overwritable_stride_limitation=[0.35, 0.4, 0, 0.35, 0.12] + gg.margin_time_ratio=0.3 + gg.use_act_states=False + gg.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE + gg.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE + gg.toe_pos_offset_x = 1e-3*117.338; + gg.heel_pos_offset_x = 1e-3*-116.342; + gg.toe_zmp_offset_x = 1e-3*117.338; + gg.heel_zmp_offset_x = 1e-3*-116.342; + gg.optional_go_pos_finalize_footstep_num=1 + gg.overwritable_footstep_index_offset=1 + self.abc_svc.setGaitGeneratorParam(gg) + def __init__(self, robotname=""): HrpsysConfigurator.__init__(self) From f889b03ac1f4b820f81ed4b1dfcb252def76f4ee Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Thu, 17 Jun 2021 00:36:58 +0900 Subject: [PATCH 15/27] add go-wheel --- hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index fc7def033..cb78c4cad 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -1289,6 +1289,10 @@ "Call goPos with wait." (send self :go-pos-no-wait xx yy th) (send self :wait-foot-steps)) + (:go-wheel + (xx tm) + "Call goWheel without wait." + (send self :autobalancerservice_goWheel :x xx :tm tm)) (:raw-get-foot-step-param () (send (send self :autobalancerservice_getfootstepparam) :i_param)) From a6a117f9254794ae933a8bb1e2ef6a0d43d73e65 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Wed, 18 Aug 2021 14:54:42 +0900 Subject: [PATCH 16/27] consider z in steppable region --- hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index e2245374d..c1943f764 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -299,10 +299,11 @@ void HrpsysSeqStateROSBridge::onSteppableRegionCB(const hrpsys_ros_bridge::Stepp m_rssteppableRegion.data.region.length(convex_num); for (size_t i = 0; i < convex_num; i++) { size_t vs_num(msg->polygons[i].polygon.points.size()); - m_rssteppableRegion.data.region[i].length(2 * vs_num); // x,y components + m_rssteppableRegion.data.region[i].length(3 * vs_num); // x,y,z components for (size_t j = 0; j < vs_num; j++) { - m_rssteppableRegion.data.region[i][2*j] = msg->polygons[i].polygon.points[j].x; - m_rssteppableRegion.data.region[i][2*j+1] = msg->polygons[i].polygon.points[j].y; + m_rssteppableRegion.data.region[i][3*j] = msg->polygons[i].polygon.points[j].x; + m_rssteppableRegion.data.region[i][3*j+1] = msg->polygons[i].polygon.points[j].y; + m_rssteppableRegion.data.region[i][3*j+2] = msg->polygons[i].polygon.points[j].z; } } m_rssteppableRegion.data.l_r = msg->l_r; @@ -1038,13 +1039,13 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id) size_t convex_num(m_currentSteppableRegion.data.region.length()); region.polygons.resize(convex_num); for (size_t i = 0; i < convex_num; i++) { - size_t vs_num(m_currentSteppableRegion.data.region[i].length()/2); + size_t vs_num(m_currentSteppableRegion.data.region[i].length()/3); if (vs_num > 3) is_valid = true; region.polygons[i].polygon.points.resize(vs_num); for (size_t j = 0; j < vs_num; j++) { - region.polygons[i].polygon.points[j].x = m_currentSteppableRegion.data.region[i][2*j]; - region.polygons[i].polygon.points[j].y = m_currentSteppableRegion.data.region[i][2*j+1]; - region.polygons[i].polygon.points[j].z = 0.0; + region.polygons[i].polygon.points[j].x = m_currentSteppableRegion.data.region[i][3*j]; + region.polygons[i].polygon.points[j].y = m_currentSteppableRegion.data.region[i][3*j+1]; + region.polygons[i].polygon.points[j].z = m_currentSteppableRegion.data.region[i][3*j+2]; } } region.l_r = m_currentSteppableRegion.data.l_r; From 4c60446dbacdfc53a9efb9f9b846df248e0d97b6 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Wed, 18 Aug 2021 17:02:45 +0900 Subject: [PATCH 17/27] fix bug on vs_num size --- hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp index c1943f764..f9ae6bb7e 100644 --- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp +++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp @@ -1040,7 +1040,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id) region.polygons.resize(convex_num); for (size_t i = 0; i < convex_num; i++) { size_t vs_num(m_currentSteppableRegion.data.region[i].length()/3); - if (vs_num > 3) is_valid = true; + if (vs_num >= 3) is_valid = true; region.polygons[i].polygon.points.resize(vs_num); for (size_t j = 0; j < vs_num; j++) { region.polygons[i].polygon.points[j].x = m_currentSteppableRegion.data.region[i][3*j]; From f0b72cbd0665f72218d5dce7e475d93a2a164c6a Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Fri, 27 Aug 2021 23:31:59 +0900 Subject: [PATCH 18/27] add go-pos-wheel --- hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index cb78c4cad..c8087e864 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -1289,6 +1289,10 @@ "Call goPos with wait." (send self :go-pos-no-wait xx yy th) (send self :wait-foot-steps)) + (:go-pos-wheel + (xx yy th w-xx w-tm) + "Call goPosWheel without wait." + (send self :autobalancerservice_goPosWheel :x xx :y yy :th th :w_x w-xx :w_tm w-tm)) (:go-wheel (xx tm) "Call goWheel without wait." From d7c584f7087224e1fe7d002c1b47e0b97a0283f0 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Sat, 2 Oct 2021 15:18:58 +0900 Subject: [PATCH 19/27] change wheel interpolation --- hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index c8087e864..720952e47 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -1294,9 +1294,9 @@ "Call goPosWheel without wait." (send self :autobalancerservice_goPosWheel :x xx :y yy :th th :w_x w-xx :w_tm w-tm)) (:go-wheel - (xx tm) + (xx rv-max ra-max) "Call goWheel without wait." - (send self :autobalancerservice_goWheel :x xx :tm tm)) + (send self :autobalancerservice_goWheel :x xx :rv_max rv-max :ra_max ra-max)) (:raw-get-foot-step-param () (send (send self :autobalancerservice_getfootstepparam) :i_param)) From 04da17f2b982bbbd9f098a21c480c0be0e89bce0 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Sat, 2 Oct 2021 17:25:01 +0900 Subject: [PATCH 20/27] change go-pos-wheel --- hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index 720952e47..418d3e72d 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -1290,9 +1290,9 @@ (send self :go-pos-no-wait xx yy th) (send self :wait-foot-steps)) (:go-pos-wheel - (xx yy th w-xx w-tm) + (xx yy th w-xx rv-max ra-max &optional (tm-off 0.0)) "Call goPosWheel without wait." - (send self :autobalancerservice_goPosWheel :x xx :y yy :th th :w_x w-xx :w_tm w-tm)) + (send self :autobalancerservice_goPosWheel :x xx :y yy :th th :w_x w-xx :rv_max rv-max :ra_max ra-max :tm_off tm-off)) (:go-wheel (xx rv-max ra-max) "Call goWheel without wait." From 26de395d17b1354fe03e979d75d5561cad373518 Mon Sep 17 00:00:00 2001 From: eus Date: Wed, 27 Oct 2021 17:20:42 +0900 Subject: [PATCH 21/27] add waitFootSteps to go-wheel --- hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index 418d3e72d..39305b279 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -1292,11 +1292,13 @@ (:go-pos-wheel (xx yy th w-xx rv-max ra-max &optional (tm-off 0.0)) "Call goPosWheel without wait." - (send self :autobalancerservice_goPosWheel :x xx :y yy :th th :w_x w-xx :rv_max rv-max :ra_max ra-max :tm_off tm-off)) + (send self :autobalancerservice_goPosWheel :x xx :y yy :th th :w_x w-xx :rv_max rv-max :ra_max ra-max :tm_off tm-off) + (send self :wait-foot-steps)) (:go-wheel (xx rv-max ra-max) "Call goWheel without wait." - (send self :autobalancerservice_goWheel :x xx :rv_max rv-max :ra_max ra-max)) + (send self :autobalancerservice_goWheel :x xx :rv_max rv-max :ra_max ra-max) + (send self :wait-foot-steps)) (:raw-get-foot-step-param () (send (send self :autobalancerservice_getfootstepparam) :i_param)) From c14f7349a1fa9984fcd11572614077a101fef28e Mon Sep 17 00:00:00 2001 From: Keitaro Murakami Date: Fri, 29 Oct 2021 00:00:32 +0900 Subject: [PATCH 22/27] [hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l] fix description of :reference-force/moment-vector --- hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index 868112e93..92f4e0471 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -117,13 +117,13 @@ (:reference-force-vector (&optional (limb)) "Returns reference force-vector [N] list for all limbs obtained by :state. - This value corresponds to StateHolder and SequencePlayer RTC. + This value corresponds to ReferenceForceUpdater, EmergencyStopper, StateHolder and SequencePlayer RTC prioritized in this order. If a limb argument is specified, returns a vector for the limb." (send self :tmp-force-moment-vector :force limb "reference")) (:reference-moment-vector (&optional (limb)) "Returns reference moment-vector [Nm] list for all limbs obtained by :state. - This value corresponds to StateHolder and SequencePlayer RTC. + This value corresponds to ReferenceForceUpdater, EmergencyStopper, StateHolder and SequencePlayer RTC prioritized in this order. If a limb argument is specified, returns a vector for the limb." (send self :tmp-force-moment-vector :moment limb "reference")) (:absolute-force-vector From c341efc2f72bc2c8421a0c4b6e487839d145f679 Mon Sep 17 00:00:00 2001 From: Keitaro Murakami Date: Fri, 29 Oct 2021 00:05:05 +0900 Subject: [PATCH 23/27] [hrpsys_ros_bridge/euslisp/datalogger-log-parser.l] fix :update-state for expecting :reference-force-vector functionality --- hrpsys_ros_bridge/euslisp/datalogger-log-parser.l | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/hrpsys_ros_bridge/euslisp/datalogger-log-parser.l b/hrpsys_ros_bridge/euslisp/datalogger-log-parser.l index 8efe7a545..11dea9994 100644 --- a/hrpsys_ros_bridge/euslisp/datalogger-log-parser.l +++ b/hrpsys_ros_bridge/euslisp/datalogger-log-parser.l @@ -207,7 +207,9 @@ (send (send self :parser-list (format nil "rmfo_off_~A" (send f :name))) :read-state)) (send self :set-robot-state1 (read-from-string (format nil ":reference-~A" (string-downcase (send f :name)))) - (send (send self :parser-list (format nil "sh_~AOut" (send f :name))) :read-state)) + (if (send self :parser-list (format nil "rfu_ref_~AOut" (send f :name))) + (send (send self :parser-list (format nil "rfu_ref_~AOut" (send f :name))) :read-state) + (send (send self :parser-list (format nil "sh_~AOut" (send f :name))) :read-state))) ) ;; (dolist (i (send robot :imu-sensors)) ;; (send self :set-robot-state1 From 360573edcb5ed43b46b6dac5ae9a0f22c665ff70 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Wed, 24 Nov 2021 16:53:12 +0900 Subject: [PATCH 24/27] change st_* to abc_* in datalogger-log-parser.l --- .../euslisp/datalogger-log-parser.l | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/hrpsys_ros_bridge/euslisp/datalogger-log-parser.l b/hrpsys_ros_bridge/euslisp/datalogger-log-parser.l index 8efe7a545..b93030a3b 100644 --- a/hrpsys_ros_bridge/euslisp/datalogger-log-parser.l +++ b/hrpsys_ros_bridge/euslisp/datalogger-log-parser.l @@ -143,25 +143,25 @@ (send self :set-robot-state1 :auto-balancer-end-coords-list (mapcar #'(lambda (x) (send robot x :end-coords :copy-worldcoords)) limb-list))) ;; Stabilizer parameter - (let ((rsd (send self :parser-list "st_originRefCogVel"))) + (let ((rsd (send self :parser-list "abc_originRefCogVel"))) (if rsd (send self :set-robot-state1 :stabilizer-reference-cogvel (scale 1e3 (send rsd :read-state))))) - (let ((rsd (send self :parser-list "st_originActCogVel"))) + (let ((rsd (send self :parser-list "abc_originActCogVel"))) (if rsd (send self :set-robot-state1 :stabilizer-cogvel (scale 1e3 (send rsd :read-state))))) - (let ((rsd (send self :parser-list "st_originRefZmp"))) + (let ((rsd (send self :parser-list "abc_originRefZmp"))) (if rsd (send self :set-robot-state1 :stabilizer-reference-zmp (scale 1e3 (send rsd :read-state))))) - (let ((rsd (send self :parser-list "st_originActZmp"))) + (let ((rsd (send self :parser-list "abc_originActZmp"))) (if rsd (send self :set-robot-state1 :stabilizer-zmp (scale 1e3 (send rsd :read-state))))) - (let ((rsd (send self :parser-list "st_originNewZmp"))) + (let ((rsd (send self :parser-list "abc_originNewZmp"))) (if rsd (send self :set-robot-state1 :stabilizer-newzmp (scale 1e3 (send rsd :read-state))))) - (let ((rsd (send self :parser-list "st_originRefCog"))) + (let ((rsd (send self :parser-list "abc_originRefCog"))) (if rsd (send self :set-robot-state1 :stabilizer-reference-cog (scale 1e3 (send rsd :read-state))))) - (let ((rsd (send self :parser-list "st_originActCog"))) + (let ((rsd (send self :parser-list "abc_originActCog"))) (if rsd (send self :set-robot-state1 :stabilizer-cog (scale 1e3 (send rsd :read-state))))) - (let ((rsd (send self :parser-list "st_actBaseRpy"))) + (let ((rsd (send self :parser-list "abc_actBaseRpy"))) (if rsd (send self :set-robot-state1 :stabilizer-act-base-rpy (map float-vector #'rad2deg (send rsd :read-state))))) - (let ((rsd (send self :parser-list "st_currentBaseRpy"))) + (let ((rsd (send self :parser-list "abc_currentBaseRpy"))) (if rsd (send self :set-robot-state1 :stabilizer-current-base-rpy (map float-vector #'rad2deg (send rsd :read-state))))) - (let ((rsd (send self :parser-list "st_debugData"))) + (let ((rsd (send self :parser-list "abc_debugData"))) (if rsd (send self :set-robot-state1 :stabilizer-debug-data (send rsd :read-state)))) ;; Set stable rtc data (send self :set-robot-state1 From e23632a4e71695631c7fe6f5ae125b7501ceccfe Mon Sep 17 00:00:00 2001 From: Keitaro Murakami Date: Fri, 26 Nov 2021 18:47:54 +0900 Subject: [PATCH 25/27] [scripts/sensor_ros_bridge_connect.py] ros topic ref_**sensor after ES --- hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py b/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py index c701fdc80..586684d09 100755 --- a/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py +++ b/hrpsys_ros_bridge/scripts/sensor_ros_bridge_connect.py @@ -28,12 +28,12 @@ def connecSensorRosBridgePort(url, rh, bridge, vs, rmfo, sh, es, rfu, subscripti print program_name, "connect ", sen.name, rmfo.port("off_" + sen.name).get_port_profile().name, bridge.port("off_" + sen.name).get_port_profile().name connectPorts(rmfo.port("off_" + sen.name), bridge.port("off_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy) # for abs forces if sen.type == 'Force' and bridge.port("ref_" + sen.name): # for reference forces - if rfu != None and rfu.port("ref_"+sen.name+"Out"): - print program_name, "connect ", sen.name, rfu.port("ref_"+sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name - connectPorts(rfu.port("ref_"+sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy) - elif es != None and es.port(sen.name+"Out"): + if es != None and es.port(sen.name+"Out"): print program_name, "connect ", sen.name, es.port(sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name connectPorts(es.port(sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy) + elif rfu != None and rfu.port("ref_"+sen.name+"Out"): + print program_name, "connect ", sen.name, rfu.port("ref_"+sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name + connectPorts(rfu.port("ref_"+sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy) elif sh.port(sen.name+"Out"): print program_name, "connect ", sen.name, sh.port(sen.name+"Out").get_port_profile().name, bridge.port("ref_" + sen.name).get_port_profile().name connectPorts(sh.port(sen.name+"Out"), bridge.port("ref_" + sen.name), subscription_type, rate=push_rate, pushpolicy=push_policy) @@ -78,4 +78,3 @@ def initSensorRosBridgeConnection(url, simulator_name, rosbridge_name, managerho initSensorRosBridgeConnection(sys.argv[1], sys.argv[2], sys.argv[3], sys.argv[4], sys.argv[5], sys.argv[6], sys.argv[7]) else : print program_name, " requires url, simulator_name, rosbridge_name" - From cc0992eb850110fc8b1a800c507ea9e3c2a0cfea Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Wed, 8 Dec 2021 17:42:57 +0900 Subject: [PATCH 26/27] add set-foot-step-with-wheel --- .../euslisp/rtm-ros-robot-interface.l | 42 +++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index 6515692d4..d30d77a2b 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -1389,6 +1389,48 @@ For arguments, please see :set-foot-steps-with-param-no-wait documentation." (send self :set-foot-steps-with-param-no-wait foot-step-list step-height-list step-time-list toe-angle-list heel-angle-list :overwrite-footstep-index overwrite-footstep-index) (send self :wait-foot-steps)) + (:set-foot-steps-with-wheel-no-wait + (foot-step-list step-height-list step-time-list toe-angle-list heel-angle-list goal-pos-list rv-max-list ra-max-list &key (overwrite-footstep-index 0)) + (unless (listp (car foot-step-list)) + (setq foot-step-list (mapcar #'(lambda (fs) (list fs)) foot-step-list) + step-height-list (mapcar #'(lambda (sh) (list sh)) step-height-list) + step-time-list (mapcar #'(lambda (st) (list st)) step-time-list) + toe-angle-list (mapcar #'(lambda (ta) (list ta)) toe-angle-list) + heel-angle-list (mapcar #'(lambda (ha) (list ha)) heel-angle-list) + goal-pos-list (mapcar #'(lambda (gp) (list gp)) goal-pos-list) + rv-max-list (mapcar #'(lambda (rv) (list rv)) rv-max-list) + ra-max-list (mapcar #'(lambda (ra) (list ra)) ra-max-list))) + (send self :autobalancerservice_setfootstepswithwheel + :fss + (mapcar #'(lambda (fs) + (instance hrpsys_ros_bridge::openhrp_autobalancerservice_footsteps :init + :fs + (mapcar #'(lambda (f) + (send self :eus-footstep->abc-footstep f)) + fs))) + foot-step-list) + :spss + (mapcar #'(lambda (shs sts tas has) + (instance hrpsys_ros_bridge::openhrp_autobalancerservice_stepparams :init + :sps + (mapcar #'(lambda (sh st ta ha) + (instance hrpsys_ros_bridge::openhrp_autobalancerservice_stepparam :init :step_height (* sh 1e-3) :step_time st :toe_angle ta :heel_angle ha)) + shs sts tas has))) + step-height-list step-time-list toe-angle-list heel-angle-list) + :wpss + (mapcar #'(lambda (gps rvs ras) + (instance hrpsys_ros_bridge::openhrp_autobalancerservice_wheelparams :init + :wps + (mapcar #'(lambda (gp rv ra) + (instance hrpsys_ros_bridge::openhrp_autobalancerservice_wheelparam :init :goal_pos (* gp 1e-3) :rv_max rv :ra_max ra)) + gps rvs ras))) + goal-pos-list rv-max-list ra-max-list) + :overwrite_fs_idx overwrite-footstep-index + )) + (:set-foot-steps-with-wheel + (foot-step-list step-height-list step-time-list toe-angle-list heel-angle-list goal-pos-list rv-max-list ra-max-list &key (overwrite-footstep-index 0)) + (send self :set-foot-steps-with-wheel-no-wait foot-step-list step-height-list step-time-list toe-angle-list heel-angle-list goal-pos-list rv-max-list ra-max-list :overwrite-footstep-index overwrite-footstep-index) + (send self :wait-foot-steps)) (:set-foot-steps-roll-pitch (angle &key (axis :x)) "Set foot steps with roll or pitch orientation. From d055b7e25933d56f0681596091b75f053eadfa34 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Fri, 17 Dec 2021 20:15:33 +0900 Subject: [PATCH 27/27] add jump-to --- hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l index 48619922b..8c99eb44f 100644 --- a/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l +++ b/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l @@ -1289,6 +1289,15 @@ "Call goPos with wait." (send self :go-pos-no-wait xx yy th) (send self :wait-foot-steps)) + (:jump-to-no-wait + (xx yy zz ts tf) + "Call jumpTo without wait." + (send self :autobalancerservice_jumpTo :x xx :y yy :z zz :ts ts :tf tf)) + (:jump-to + (xx yy zz ts tf) + "Call jumpTo with wait." + (send self :jump-to-no-wait xx yy zz ts tf) + (send self :wait-foot-steps)) (:raw-get-foot-step-param () (send (send self :autobalancerservice_getfootstepparam) :i_param))