0
$\begingroup$

I am developing heaped project. I am using ROS2 humble with JointTrajectoryController.

When I spawn model in gazebo it moves uncontrollably, it shoot itself out of sight. When it lands on its back it stays put. Whenever one of its legs touches ground it is all over. When I spawn model without controller model behaves as expected (just folds itself due to gravity). Here is a gif (its shaky, because i am following the model, because it throws itself out of reach of standard view).

I tried changing damping, friction, PID, nothing works. Here is part of my urdf and controllers.yaml:

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <link name="base_link">
    </link>
    <link name="body_link">
        <inertial>
            <origin xyz="0.160055124 0.267586317 0.39836396" rpy="0 0 0" />
            <mass value="0.64954282" />
            <inertia ixx="0.000794907" ixy="4.01E-09" ixz="-4.37E-08" iyy="0.001704166" iyz="4.77E-07" izz="0.001070923" />
        </inertial>
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename=".../body_link.STL" />
            </geometry>
            <material name="">
                <color rgba="1 1 1 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename="file:.../body_link.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="body_joint" type="fixed">
        <origin xyz="-0.102475952 0.159586753 -0.131089911" rpy="1.570796327 0 0" />
        <parent link="base_link" />
        <child link="body_link" />
        <axis xyz="0 0 0" />
        <dynamics damping="0.0" friction="0.0" />
    </joint>
    <link name="coxa_fl_link">
        <inertial>
            <origin xyz="-0.031263 -0.04285 -0.0031807" rpy="0 0 0" />
            <mass value="0.060087" />
            <inertia ixx="1.33E-05" ixy="-7.73E-08" ixz="3.75E-08" iyy="2.61E-05" iyz="-4.96E-07" izz="2.23E-05" />
        </inertial>
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename=".../coxa_fl_link.STL" />
            </geometry>
            <material name="">
                <color rgba="1 1 1 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <mesh filename=".../coxa_fl_link.STL" />
            </geometry>
        </collision>
    </link>
    <joint name="coxa_fl_joint" type="revolute">
        <origin xyz="0.25338 0.24361 0.50425" rpy="-2.618 0 -1.5708" />
        <parent link="body_link" />
        <child link="coxa_fl_link" />
        <axis xyz="1 0 0" />
        <limit lower="-1.5" upper="1.5" effort="0.1" velocity="0.1" />
        <dynamics damping="0.0" friction="0.0" />
    </joint>
    <link name="femur_fl_link">
...
controller_manager:
  ros__parameters:
    update_rate: 30
    # use_sim_time: true

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    joint_trajectory_position_controller:
      type: joint_trajectory_controller/JointTrajectoryController

# joint_state_broadcaster:
#   ros__parameters:
    
joint_trajectory_position_controller:
  ros__parameters:
    joints:
      - coxa_fl_joint
      - femur_fl_joint
      - tibia_fl_joint
      - coxa_ml_joint
      - femur_ml_joint
      - tibia_ml_joint
      - coxa_rl_joint
      - femur_rl_joint
      - tibia_rl_joint
      - coxa_fr_joint
      - femur_fr_joint
      - tibia_fr_joint
      - coxa_mr_joint
      - femur_mr_joint
      - tibia_mr_joint
      - coxa_rr_joint
      - femur_rr_joint
      - tibia_rr_joint

    gains:
      coxa_fl_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      femur_fl_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      tibia_fl_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      coxa_ml_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      femur_ml_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      tibia_ml_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      coxa_rl_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      femur_rl_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      tibia_rl_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      coxa_fr_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      femur_fr_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      tibia_fr_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      coxa_mr_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      femur_mr_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      tibia_mr_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      coxa_rr_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      femur_rr_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}
      tibia_rr_joint: {p: 0.0, i: 0.0, d: 0.0, i_clamp: 0.0, ff_velocity_scale: 0.00}

    
    command_interfaces:
      - position

    state_interfaces:
      - position
      # - velocity

    action_monitor_rate: 20.0 # Defaults to 20

    allow_partial_joints_goal: false # Defaults to false
    open_loop_control: true
    allow_integration_in_goal_trajectories: false
    constraints:
      stopped_velocity_tolerance: 0.01 # Defaults to 0.01
      goal_time: 0.0 # Defaults to 0.0 (start immediately)

$\endgroup$

2 Answers 2

2
$\begingroup$

The issue was with inertia. The inertia was too small and some values were negative. I increased inertia of all the links and it started behaving normally.

$\endgroup$
0
$\begingroup$

Do you initialize the JTC with any commands, or just activate it and leave it there? The current released version for humble should not send any commands to the hardware/simulation until the first trajectory is received. Can you try to compile this version and test if there is any difference?

$\endgroup$
2
  • $\begingroup$ I just activate it and leave it there. $\endgroup$ Nov 19, 2023 at 19:20
  • $\begingroup$ Tried new version with same result $\endgroup$ Nov 19, 2023 at 21:26

Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.