0
$\begingroup$

Rosanswers logo

Hi, I am trying to reset a model to it's original position but when I call the "/reset_world" service (without a large time delay with time.sleep) the model bounces in the air. Any help to prevent this bounce/ wobble would be appreciated as many resets will be needed in the simulation (after every episode for Q-Learning) so it would save a lot of time. (Below is the code for controlling the model and resetting the simulation as well as the URDF)

for episode in range(5000):
        
        episode_rewards = 0
        rclpy.spin_once(self.imu_sub)
        self.observation = self.imu_sub.get_latest_observation()
        while(not self.is_episode_finished()):
            current_state = self.discretise_observation()
            action = self.choose_action(current_state)
            
            self.cmd_vel_pub.publish(action) #move_robot()
            rclpy.spin_once(self.imu_sub)
            self.observation = self.imu_sub.get_latest_observation()

            new_state = self.discretise_observation()
            reward = self.get_reward(new_state[0])
            episode_rewards += reward
            lr = self.get_learning_rate(episode+1)
            self.Q_table[current_state][action] = self.Q_table[current_state][action] + lr * (reward + self.DISCOUNT_FACTOR * np.max(self.Q_table[new_state]) - self.Q_table[current_state][action])
        
        
        self.get_logger().info(f"{episode}| Episode rewards: {episode_rewards}")
        self.epsilon = max(0.01, self.epsilon * self.epsilon_decay)
        self.all_rewards.append(episode_rewards)
        self.resetSim.send_request()

class ResetSim(Node):
def __init__(self):
    super().__init__("minimum_reset_client")
    self.client = self.create_client(Empty, "/reset_world")
    while not self.client.wait_for_service():
        self.get_logger().info("Service not available, waiting again...")
    self.req = Empty.Request()

def send_request(self):
    self.future = self.client.call_async(self.req)
    rclpy.spin_until_future_complete(self, self.future)
    return self.future.result()




<robot name="m2wr" xmlns:xacro="https://www.ros.org/wiki/xacro" >
    
<link name="link_chassis">
    <!-- pose and inertial -->
    <pose>0 0 0 0 0 0</pose>
    
    <inertial>
    <mass value="5"/>
    <origin rpy="0 0 0" xyz="0 0 0.1"/>
    <inertia ixx="0.0395416666667" ixy="0" ixz="0" iyy="0.106208333333" iyz="0" izz="0.106208333333"/>
    </inertial>
    
    <collision name="collision_chassis">
    <geometry>
        <box size="0.5 0.5 0.8"/>
    </geometry>
    </collision>
    
    <visual>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <geometry>
        <box size="0.5 0.5 0.8"/>
    </geometry>
    <material name="blue"/>
    </visual>
    </link>

<!-- Create wheel right -->  
    
<link name="link_right_wheel">    
    <inertial>
    <mass value="0.2"/>
    <origin rpy="0 1.5707 1.5707" xyz="0 0 0"/>
    <inertia ixx="0.00052666666" ixy="0" ixz="0" iyy="0.00052666666" iyz="0" izz="0.001"/>
    </inertial>
    
    <collision name="link_right_wheel_collision">
    <origin rpy="0 1.5707 1.5707" xyz="0 0 0" />
    <geometry>
        <cylinder length="0.1" radius="0.2"/>
    </geometry>
    </collision>
    
    <visual name="link_right_wheel_visual">
    <origin rpy="0 1.5707 1.5707" xyz="0 0 0"/>
    <geometry>
        <cylinder length="0.1" radius="0.2"/>
    </geometry>      
    </visual>
    
</link>
    
<!--   Joint for right wheel  -->
<joint name="joint_right_wheel" type="continuous">
    <origin rpy="0 0 0" xyz="0 0.3 -0.4"/>
    <child link="link_right_wheel" />
    <parent link="link_chassis"/>
    <axis rpy="0 0 0" xyz="0 -1 0"/>
    <limit effort="10000" velocity="1000"/>
    <joint_properties damping="1.0" friction="1.0" />
</joint>  
    
<!-- Left Wheel link -->
    
<link name="link_left_wheel">    
    <inertial>
    <mass value="0.2"/>
    <origin rpy="0 1.5707 1.5707" xyz="0 0 0"/>
    <inertia ixx="0.00052666666" ixy="0" ixz="0" iyy="0.00052666666" iyz="0" izz="0.001"/>
    </inertial>
    
    <collision name="link_left_wheel_collision">
    <origin rpy="0 1.5707 1.5707" xyz="0 0 0" />
    <geometry>
        <cylinder length="0.1" radius="0.2"/>
    </geometry>
    </collision>
    
    <visual name="link_left_wheel_visual">
    <origin rpy="0 1.5707 1.5707" xyz="0 0 0"/>
    <geometry>
        <cylinder length="0.1" radius="0.2"/>
    </geometry>      
    </visual>
    
</link>
    
<!--   Joint for right wheel  -->
<joint name="joint_left_wheel" type="continuous">
    <origin rpy="0 0 0" xyz="0 -0.3 -0.4"/>
    <child link="link_left_wheel" />
    <parent link="link_chassis"/>
    <axis rpy="0 0 0" xyz="0 1 0"/>
    <limit effort="10000" velocity="1000"/>
    <joint_properties damping="1.0" friction="1.0" />
</joint> 



    
<gazebo>
    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
    <update_rate>20</update_rate>
    <left_joint>joint_left_wheel</left_joint>
    <right_joint>joint_right_wheel</right_joint>
    <wheel_separation>0.4</wheel_separation>
    <wheel_diameter>0.2</wheel_diameter>
    <wheel_torque>0.1</wheel_torque>
    <command_topic>cmd_vel</command_topic>
    <odometry_topic>odom</odometry_topic>
    <odometry_frame>odom</odometry_frame>
    <robot_base_frame>link_chassis</robot_base_frame>
    </plugin>
    
    

</gazebo>
<gazebo reference="link_chassis">
    <sensor name="my_imu" type="imu">
        <always_on>true</always_on>
        <!-- Publish at 30 hz -->
        <update_rate>30</update_rate>
        <plugin name="my_imu_plugin" filename="libgazebo_ros_imu_sensor.so">
        <ros>
        <!-- Will publish to /imu/data -->
        <namespace>/imu</namespace>
        <remapping>~/out:=data</remapping>
        </ros>
        <frame_name>link_chassis</frame_name>
        <initial_orientation_as_reference>false</initial_orientation_as_reference>
        </plugin>
    </sensor>
</gazebo>

Originally posted by vertical_beef576 on ROS Answers with karma: 17 on 2023-07-24

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

By wobble/bounce, do you mean a small movement at the robot's start position? If that is the case, it is probably because the offset from ground maybe too low, and the wheel of the robot rubs against the ground_plane of the world, causing the weight shift.

Node(      
    package='gazebo_ros', executable='spawn_entity.py',
    arguments=[
      '-topic', 'robot_description',
      '-entity', 'm2wr',
      '-x', '0',
      '-y', '0',
      '-z', '0.0' # No offset
    ],
    output='screen'
),

There is indeed a small bounce/wobble when you try calling the service with no offset

To combat this, you could spawn the robot at a small offset from the origin (say 5cm):

Node(
    package='gazebo_ros', executable='spawn_entity.py',
    arguments=[
      '-topic', 'robot_description',
      '-entity', 'm2wr',
      '-x', '0',
      '-y', '0',
      '-z', '0.05' # Minimal offset
    ],
    output='screen'
),

As indicated above, spawning at a minimal offset removes all bounce.

Note

  • If you want to reset only the robot without resetting all the poses of the models in the simulation, you can use the /set_entity_state service after adding the following block to your world file:

    Snippet to be added to your world:

      <plugin name="gazebo_ros_state" filename="libgazebo_ros_state.so">
        <ros>
          <namespace>/</namespace>
          <remapping>model_states:=model_states_demo</remapping>
          <remapping>link_states:=link_states_demo</remapping>
        </ros>
        <update_rate>50.0</update_rate>
      </plugin>
    

ROS2 Service Call:

    ros2 service call /set_entity_state gazebo_msgs/SetEntityState "state: {name: m2wr, pose: {position: {x: 0.0, y: 0.0, z: 0.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, reference_frame: world}"


Originally posted by Gaurav Gupta with karma: 276 on 2023-07-26

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.