diff --git a/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py b/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py index 5263efe..edfcef7 100644 --- a/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py +++ b/controllers/ocs2_quadruped_controller/launch/gazebo.launch.py @@ -128,6 +128,8 @@ def generate_launch_description(): executable="parameter_bridge", arguments=[ "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock", + "/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry", + "/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V" ], output="screen" ) diff --git a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp index 1ac80fc..533aa1b 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp @@ -36,7 +36,7 @@ namespace ocs2::legged_robot { void StateEstimateBase::updateContact() { const size_t size = ctrl_component_.foot_force_state_interface_.size(); for (int i = 0; i < size; i++) { - contact_flag_[i] = ctrl_component_.foot_force_state_interface_[i].get().get_value() > 10.0; + contact_flag_[i] = ctrl_component_.foot_force_state_interface_[i].get().get_value() > 5.0; } } diff --git a/descriptions/deep_robotics/lite3_description/README.md b/descriptions/deep_robotics/lite3_description/README.md index f2b0d00..6365747 100644 --- a/descriptions/deep_robotics/lite3_description/README.md +++ b/descriptions/deep_robotics/lite3_description/README.md @@ -59,4 +59,16 @@ ros2 launch lite3_description visualize.launch.py ```bash source ~/ros2_ws/install/setup.bash ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=lite3_description height:=0.43 - ``` \ No newline at end of file + ``` +* OCS2 Quadruped Controller + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch ocs2_quadruped_controller gazebo.launch.py pkg_description:=lite3_description height:=0.43 + ``` + +### Gazebo Playground (ROS2 Jazzy) +* OCS2 Quadruped Controller + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch gz_quadruped_playground gazebo.launch.py pkg_description:=lite3_description controller:=ocs2 world:=warehouse + ``` \ No newline at end of file diff --git a/descriptions/deep_robotics/lite3_description/config/gazebo.yaml b/descriptions/deep_robotics/lite3_description/config/gazebo.yaml index 258bc12..63a9e01 100644 --- a/descriptions/deep_robotics/lite3_description/config/gazebo.yaml +++ b/descriptions/deep_robotics/lite3_description/config/gazebo.yaml @@ -10,12 +10,12 @@ controller_manager: imu_sensor_broadcaster: type: imu_sensor_broadcaster/IMUSensorBroadcaster - leg_pd_controller: - type: leg_pd_controller/LegPdController - unitree_guide_controller: type: unitree_guide_controller/UnitreeGuideController + ocs2_quadruped_controller: + type: ocs2_quadruped_controller/Ocs2QuadrupedController + rl_quadruped_controller: type: rl_quadruped_controller/LeggedGymController @@ -24,36 +24,70 @@ imu_sensor_broadcaster: sensor_name: "imu_sensor" frame_id: "imu_link" -leg_pd_controller: +ocs2_quadruped_controller: ros__parameters: - update_rate: 1000 # Hz + update_rate: 200 # Hz + robot_pkg: "lite3_description" + default_kd: 2.0 joints: - - FR_HipX - - FR_HipY - - FR_Knee - FL_HipX - FL_HipY - FL_Knee - - HR_HipX - - HR_HipY - - HR_Knee + - FR_HipX + - FR_HipY + - FR_Knee - HL_HipX - HL_HipY - HL_Knee + - HR_HipX + - HR_HipY + - HR_Knee command_interfaces: - effort - - state_interfaces: - position - velocity + - kp + - kd + + state_interfaces: + - effort + - position + - velocity + + feet: + - FL_FOOT + - FR_FOOT + - HL_FOOT + - HR_FOOT + imu_name: "imu_sensor" + base_name: "base" + + imu_interfaces: + - orientation.w + - orientation.x + - orientation.y + - orientation.z + - angular_velocity.x + - angular_velocity.y + - angular_velocity.z + - linear_acceleration.x + - linear_acceleration.y + - linear_acceleration.z + +# estimator_type: "odom" + foot_force_name: "foot_force" + foot_force_interfaces: + - FL_foot_force + - HL_foot_force + - FR_foot_force + - HR_foot_force unitree_guide_controller: ros__parameters: update_rate: 200 # Hz # stand_kp: 30.0 stand_kd: 1.0 - command_prefix: "leg_pd_controller" joints: - FR_HipX - FR_HipY diff --git a/descriptions/deep_robotics/lite3_description/config/ocs2/reference.info b/descriptions/deep_robotics/lite3_description/config/ocs2/reference.info index 0447bde..9b2f52a 100644 --- a/descriptions/deep_robotics/lite3_description/config/ocs2/reference.info +++ b/descriptions/deep_robotics/lite3_description/config/ocs2/reference.info @@ -1,22 +1,22 @@ targetDisplacementVelocity 0.5; targetRotationVelocity 1.57; -comHeight 0.39 +comHeight 0.3; defaultJointState { (0,0) 0.1 ; FL_HipX - (1,0) -0.732 ; FL_HipY - (2,0) 1.361 ; FL_Knee + (1,0) -0.985 ; FL_HipY + (2,0) 2.084 ; FL_Knee (3,0) -0.1 ; FR_HipX - (4,0) -0.732 ; FR_HipY - (5,0) 1.361 ; FR_Knee + (4,0) -0.985 ; FR_HipY + (5,0) 2.084 ; FR_Knee (6,0) 0.1 ; HL_HipX - (7,0) -0.732 ; HL_HipY - (8,0) 1.361 ; HL_Knee + (7,0) -0.985 ; HL_HipY + (8,0) 2.084 ; HL_Knee (9,0) -0.1 ; HR_HipX - (10,0) -0.732 ; HR_HipY - (11,0) 1.361 ; HR_Knee + (10,0) -0.985 ; HR_HipY + (11,0) 2.084 ; HR_Knee } initialModeSchedule diff --git a/descriptions/deep_robotics/lite3_description/config/ocs2/task.info b/descriptions/deep_robotics/lite3_description/config/ocs2/task.info index 6a68b93..6664796 100644 --- a/descriptions/deep_robotics/lite3_description/config/ocs2/task.info +++ b/descriptions/deep_robotics/lite3_description/config/ocs2/task.info @@ -154,24 +154,24 @@ initialState ;; Base Pose: [position, orientation] ;; (6,0) 0.0 ; p_base_x (7,0) 0.0 ; p_base_y - (8,0) 0.39 ; p_base_z + (8,0) 0.3 ; p_base_z (9,0) 0.0 ; theta_base_z (10,0) 0.0 ; theta_base_y (11,0) 0.0 ; theta_base_x ;; Leg Joint Positions: [FL, HL, FR, HR] ;; (12,0) 0.1 ; FL_HipX - (13,0) -0.732 ; FL_HipY - (14,0) 1.361 ; FL_Knee + (13,0) -0.985 ; FL_HipY + (14,0) 2.084 ; FL_Knee (15,0) 0.1 ; HL_HipX - (16,0) -0.732 ; HL_HipY - (17,0) 1.361 ; HL_Knee + (16,0) -0.985 ; HL_HipY + (17,0) 2.084 ; HL_Knee (18,0) -0.1 ; FR_HipX - (19,0) -0.732 ; FR_HipY - (20,0) 1.361 ; FR_Knee + (19,0) -0.985 ; FR_HipY + (20,0) 2.084 ; FR_Knee (21,0) -0.1 ; HR_HipX - (22,0) -0.732 ; HR_HipY - (23,0) 1.361 ; HR_Knee + (22,0) -0.985 ; HR_HipY + (23,0) 2.084 ; HR_Knee } ; standard state weight matrix @@ -180,12 +180,12 @@ Q scaling 1e+0 ;; Normalized Centroidal Momentum: [linear, angular] ;; - (0,0) 15.0 ; vcom_x - (1,1) 15.0 ; vcom_y - (2,2) 100.0 ; vcom_z - (3,3) 10.0 ; L_x / robotMass - (4,4) 30.0 ; L_y / robotMass - (5,5) 30.0 ; L_z / robotMass + (0,0) 10.0 ; vcom_x + (1,1) 10.0 ; vcom_y + (2,2) 80.0 ; vcom_z + (3,3) 8.0 ; L_x / robotMass + (4,4) 20.0 ; L_y / robotMass + (5,5) 20.0 ; L_z / robotMass ;; Base Pose: [position, orientation] ;; (6,6) 1000.0 ; p_base_x @@ -295,8 +295,8 @@ frictionConeTask swingLegTask { - kp 350 - kd 37 + kp 300 + kd 20 } weight diff --git a/descriptions/deep_robotics/lite3_description/xacro/body.xacro b/descriptions/deep_robotics/lite3_description/xacro/body.xacro deleted file mode 100644 index 520b856..0000000 --- a/descriptions/deep_robotics/lite3_description/xacro/body.xacro +++ /dev/null @@ -1,451 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/descriptions/deep_robotics/lite3_description/xacro/const.xacro b/descriptions/deep_robotics/lite3_description/xacro/const.xacro new file mode 100644 index 0000000..bee270d --- /dev/null +++ b/descriptions/deep_robotics/lite3_description/xacro/const.xacro @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/descriptions/deep_robotics/lite3_description/xacro/gazebo.xacro b/descriptions/deep_robotics/lite3_description/xacro/gazebo.xacro index 5bdd9a3..2b828c1 100644 --- a/descriptions/deep_robotics/lite3_description/xacro/gazebo.xacro +++ b/descriptions/deep_robotics/lite3_description/xacro/gazebo.xacro @@ -1,90 +1,151 @@ + - gz_ros2_control/GazeboSimSystem + gz_quadruped_hardware/GazeboSimSystem - + + + + + + - + + + + + + - + + + + + + - + + + + + + - + + + + + + - + + + + + + - + + + + + + - + + + + + + - + + + + + + - + + + + + + - + + + + + + - + + + + + + @@ -102,15 +163,34 @@ + + + + + + + + - + $(find lite3_description)/config/gazebo.yaml + + + + + + + + + + + @@ -128,61 +208,82 @@ - - - - - - + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + - - - - - - + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + - - - - - - + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + - - - - - - + + 0.0 + 1.7e-2 + 0.1 + 0.001 + - - - - - - + + 0.0 + 1.7e-2 + 0.1 + 0.001 + - - - - - - + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + true + + + + + + + + + + + + + + + + + + + + diff --git a/descriptions/deep_robotics/lite3_description/xacro/imu_link.xacro b/descriptions/deep_robotics/lite3_description/xacro/imu_link.xacro deleted file mode 100644 index b90aa0a..0000000 --- a/descriptions/deep_robotics/lite3_description/xacro/imu_link.xacro +++ /dev/null @@ -1,30 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/descriptions/deep_robotics/lite3_description/xacro/leg.xacro b/descriptions/deep_robotics/lite3_description/xacro/leg.xacro new file mode 100644 index 0000000..c378f98 --- /dev/null +++ b/descriptions/deep_robotics/lite3_description/xacro/leg.xacro @@ -0,0 +1,164 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.2 + + + + 0.2 + 0.2 + 0 + + + + + + 0.6 + 0.6 + 1 + + + + 0.6 + 0.6 + 1 + + + + + + + + + diff --git a/descriptions/deep_robotics/lite3_description/xacro/robot.xacro b/descriptions/deep_robotics/lite3_description/xacro/robot.xacro index b2a22a4..bb1700b 100644 --- a/descriptions/deep_robotics/lite3_description/xacro/robot.xacro +++ b/descriptions/deep_robotics/lite3_description/xacro/robot.xacro @@ -2,8 +2,8 @@ - - + + @@ -21,4 +21,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/descriptions/deep_robotics/lite3_description/xacro/transmission.xacro b/descriptions/deep_robotics/lite3_description/xacro/transmission.xacro index c1769d2..ddd5ed5 100644 --- a/descriptions/deep_robotics/lite3_description/xacro/transmission.xacro +++ b/descriptions/deep_robotics/lite3_description/xacro/transmission.xacro @@ -2,128 +2,34 @@ - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + diff --git a/descriptions/unitree/go2_description/README.md b/descriptions/unitree/go2_description/README.md index 4bdb7c2..d2af45e 100644 --- a/descriptions/unitree/go2_description/README.md +++ b/descriptions/unitree/go2_description/README.md @@ -61,6 +61,11 @@ ros2 launch go2_description visualize.launch.py source ~/ros2_ws/install/setup.bash ros2 launch unitree_guide_controller gazebo.launch.py ``` +* OCS2 Quadruped Controller + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch ocs2_quadruped_controller gazebo.launch.py + ``` * RL Quadruped Controller ```bash source ~/ros2_ws/install/setup.bash diff --git a/descriptions/unitree/go2_description/xacro/const.xacro b/descriptions/unitree/go2_description/xacro/const.xacro index 3d8cbb9..5436b5c 100755 --- a/descriptions/unitree/go2_description/xacro/const.xacro +++ b/descriptions/unitree/go2_description/xacro/const.xacro @@ -51,7 +51,7 @@ - + diff --git a/descriptions/unitree/go2_description/xacro/gazebo.xacro b/descriptions/unitree/go2_description/xacro/gazebo.xacro index 8980b0a..272382a 100644 --- a/descriptions/unitree/go2_description/xacro/gazebo.xacro +++ b/descriptions/unitree/go2_description/xacro/gazebo.xacro @@ -169,16 +169,6 @@ name="gz::sim::systems::Imu"> - - odom - base - 1000 - odom - 3 - odom_with_covariance - tf - @@ -254,10 +244,10 @@ true - - - - + + + + diff --git a/hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro b/hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro index e179369..d09d0c6 100644 --- a/hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro +++ b/hardwares/gz_quadruped_hardware/xacro/foot_force_sensor.xacro @@ -1,8 +1,8 @@ - - + + true diff --git a/libraries/gz_quadruped_playground/config/ocs2.rviz b/libraries/gz_quadruped_playground/config/ocs2.rviz new file mode 100644 index 0000000..3a67d37 --- /dev/null +++ b/libraries/gz_quadruped_playground/config/ocs2.rviz @@ -0,0 +1,599 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 270 + Name: Displays + Property Tree Widget: + Expanded: + - /TF1/Frames1 + - /Target Trajectories1/Target Feet Trajectories1/Marker1 + - /Target Trajectories1/Target Feet Trajectories1/Marker2 + - /Target Trajectories1/Target Feet Trajectories1/Marker3 + - /Target Trajectories1/Target Feet Trajectories1/Marker4 + - /Target Trajectories1/Target Base Trajectory1 + - /PointCloud22 + Splitter Ratio: 0.5 + Tree Height: 638 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 + - Class: rviz_common/Displays + Help Height: 138 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.8294117450714111 + Tree Height: 268 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 0; 0; 0 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: odom + Value: true + - Alpha: 0.800000011920929 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + FL_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FL_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + FR_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RL_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RL_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_calf: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_foot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_hip: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RR_thigh: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_d435: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lidar: + Alpha: 1 + Show Axes: false + Show Trail: false + trunk: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: false + FL_calf: + Value: true + FL_foot: + Value: true + FL_hip: + Value: true + FL_thigh: + Value: true + FR_calf: + Value: true + FR_foot: + Value: true + FR_hip: + Value: true + FR_thigh: + Value: true + RL_calf: + Value: true + RL_foot: + Value: true + RL_hip: + Value: true + RL_thigh: + Value: true + RR_calf: + Value: true + RR_foot: + Value: true + RR_hip: + Value: true + RR_thigh: + Value: true + base: + Value: false + camera_d435: + Value: true + front_camera: + Value: true + imu_link: + Value: false + lidar: + Value: true + odom: + Value: true + trunk: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: false + Show Names: false + Tree: + odom: + base: + trunk: + FL_hip: + FL_thigh: + FL_calf: + FL_foot: + {} + FR_hip: + FR_thigh: + FR_calf: + FR_foot: + {} + RL_hip: + RL_thigh: + RL_calf: + RL_foot: + {} + RR_hip: + RR_thigh: + RR_calf: + RR_foot: + {} + camera_d435: + {} + front_camera: + {} + imu_link: + {} + lidar: + {} + Update Interval: 0 + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Optimized State Trajectory + Namespaces: + CoM Trajectory: true + EE Trajectories: true + Future footholds: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /legged_robot/optimizedStateTrajectory + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Current State + Namespaces: + Center of Pressure: true + EE Forces: true + EE Positions: true + Support Polygon: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /legged_robot/currentState + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /legged_robot/desiredFeetTrajectory/LF + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /legged_robot/desiredFeetTrajectory/LH + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /legged_robot/desiredFeetTrajectory/RF + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /legged_robot/desiredFeetTrajectory/RH + Value: true + Enabled: false + Name: Target Feet Trajectories + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Target Base Trajectory + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /legged_robot/desiredBaseTrajectory + Value: true + Enabled: true + Name: Target Trajectories + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + Enabled: true + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /raisim_heightmap + Use Rainbow: true + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /camera/image + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /rgbd_d435/points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 198; 198; 198 + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.071281671524048 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -1.0683034658432007 + Y: -0.7991123795509338 + Z: 0.4058437943458557 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.18039782345294952 + Target Frame: trunk + Value: Orbit (rviz_default_plugins) + Yaw: 3.5117714405059814 + Saved: + - Class: rviz_default_plugins/Orbit + Distance: 2.071281671524048 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -1.0683034658432007 + Y: -0.7991123795509338 + Z: 0.4058437943458557 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.009999999776482582 + Pitch: 0.46039777994155884 + Target Frame: trunk + Value: Orbit (rviz_default_plugins) + Yaw: 3.326773166656494 +Window Geometry: + Displays: + collapsed: false + Height: 1182 + Hide Left Dock: true + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f4000003fefc020000000cfb0000001200530065006c0065006300740069006f006e000000006e000000b0000000b200fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000006e00000477000000b200fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000070000003fe0000018600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001be000001900000000000000000fb000000100044006900730070006c006100790073000000006e000002470000018600fffffffb0000000a0049006d0061006700650000000070000002990000002800fffffffb0000000a0049006d0061006700650100000395000000d90000000000000000000000010000015d000003fefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000070000003fe0000013800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d00650000000000000006400000047a00fffffffb0000000800540069006d006501000000000000045000000000000000000000070f000003fe00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1807 + X: 2248 + Y: 914 diff --git a/libraries/gz_quadruped_playground/launch/gazebo.launch.py b/libraries/gz_quadruped_playground/launch/gazebo.launch.py index cb31a09..1b2ea43 100644 --- a/libraries/gz_quadruped_playground/launch/gazebo.launch.py +++ b/libraries/gz_quadruped_playground/launch/gazebo.launch.py @@ -51,15 +51,6 @@ def launch_setup(context, *args, **kwargs): ], ) - rviz_config_file = os.path.join(get_package_share_directory('gz_quadruped_playground'), "config", "rviz.rviz") - rviz = Node( - package='rviz2', - executable='rviz2', - name='rviz', - output='screen', - arguments=["-d", rviz_config_file] - ) - # Controllers controller = context.launch_configurations['controller'] if controller == 'ocs2': @@ -68,12 +59,22 @@ def launch_setup(context, *args, **kwargs): 'launch', 'ocs2.launch.py'])]) ) + rviz_config_file = os.path.join(get_package_share_directory('gz_quadruped_playground'), "config", "ocs2.rviz") else: controller_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([PathJoinSubstitution([FindPackageShare('gz_quadruped_playground'), 'launch', 'unitree_guide.launch.py'])]) ) + rviz_config_file = os.path.join(get_package_share_directory('gz_quadruped_playground'), "config", "rviz.rviz") + + rviz = Node( + package='rviz2', + executable='rviz2', + name='rviz', + output='screen', + arguments=["-d", rviz_config_file] + ) return [ rviz, diff --git a/libraries/gz_quadruped_playground/models/D435/model.xacro b/libraries/gz_quadruped_playground/models/D435/model.xacro index 1074711..137dfc7 100644 --- a/libraries/gz_quadruped_playground/models/D435/model.xacro +++ b/libraries/gz_quadruped_playground/models/D435/model.xacro @@ -4,7 +4,7 @@ - + @@ -77,7 +77,7 @@ camera_${name} true - 15 + 30 true rgbd_${name} + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + scan + 10 + lidar_${name} + + + + 640 + 1 + 0 + 6.28 + + + ${vertical} + 1 + -0.261799 + 0.261799 + + + + 0.5 + 10.0 + 0.01 + + + true + true + + + + \ No newline at end of file diff --git a/libraries/gz_quadruped_playground/worlds/baylands.sdf b/libraries/gz_quadruped_playground/worlds/baylands.sdf new file mode 100644 index 0000000..8f1c52d --- /dev/null +++ b/libraries/gz_quadruped_playground/worlds/baylands.sdf @@ -0,0 +1,69 @@ + + + + + 0.001 + 1.0 + + + ogre2 + + + + + + + + + + 0.8 0.5 1 + false + + true + + 1 + + + 0 0 500 0 -0 0 + false + 1 + 0.001 0.625 -0.78 + 0.904 0.904 0.904 1 + 0.271 0.271 0.271 1 + + 2000 + 0 + 1 + 0 + + + + + https://fuel.gazebosim.org/1.0/saurav/models/simple_baylands + + park + 248 200 -1 0 0 -0.45 + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Coast Water + + 0 0 -2 0 0 0 + + + EARTH_WGS84 + ENU + 37.412173071650805 + -121.998878727967 + 38 + + +