initial joint of the robot

[Nash-dc]

Hi Mr.Giang, I follow the tutorial to build my own franka robot,but there is a problem when I try to initiate the pose of the panda Screencast from 18.09.2023 18:57:22.webm you can see that the robot is first initialized with the joint_inits, but soon the pose is changed and the arm keep shaking.I haven't had this problem running you pick_and_place demo before. I think I might have the problem with the yaml file the following is my joint_inits

mujoco:
  joint_inits:
    panda_2_joint1: 0
    panda_2_joint2: -0.785398163397
    panda_2_joint3: 0
    panda_2_joint4: -2.35619449019
    panda_2_joint5: 0
    panda_2_joint6: 1.57079632679
    panda_2_joint7: 0.785398163397

  root_frame_id: map

multiverse:  
  length_unit: m
  angle_unit: rad
  force_unit: N
  time_unit: s
  handedness: rhs
  root_frame_id: map

  send:
    joint: [joint_rvalue, joint_tvalue]
  receive:
    StaticMeshActor_3: [position, quaternion]

Here is my launch file:

<launch>
    <arg name="namespace" default="dual_panda" />

    <group ns="$(arg namespace)">
        <include file="$(find mujoco_sim)/launch/mujoco_sim.launch">
            <arg name="config" value="$(find franka_mujoco)/config/dual_panda.yaml" />
            <arg name="world" value="$(find mujoco_sim)/model/world/empty.xml" />
            <arg name="robot" value="$(find franka_description)/robots/dual_panda_arm.urdf" />
            <arg name="use_urdf" value="true" />
            <arg name="socket_port" value="7500" />
        </include>
    </group>

    <group ns="$(arg namespace)">
        <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

        <rosparam command="load" file="$(find franka_control)/config/dual_panda_arm_controllers.yaml" />
        <node name="controller_manager" pkg="controller_manager" type="controller_manager" respawn="false" output="screen" args="spawn 
        joint_state_controller 
        panda_2_joint_trajectory_controller 
        panda_2_gripper_controller" /> 
        <node name="rqt_joint_trajectory_controller" pkg="rqt_joint_trajectory_controller" type="rqt_joint_trajectory_controller" />
    </group>

    <include file="$(find giskardpy)/launch/giskardpy_dual_panda.launch" />
    <group ns="$(arg namespace)">
        <node pkg="franka_control" type="test_mimic_panda_1.py" name="test_1"
            output="screen" launch-prefix="bash -c 'sleep 5; $0 $@' " />

    </group>

</launch>

Here is my dual_panda_arm_controllers.yaml

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 60

panda_2_joint_trajectory_controller:
  type: effort_controllers/JointTrajectoryController
  joints:
    - panda_2_joint1
    - panda_2_joint2
    - panda_2_joint3
    - panda_2_joint4
    - panda_2_joint5
    - panda_2_joint6
    - panda_2_joint7
  gains:
    panda_2_joint1: {p: 2000, i: 100, d: 50}
    panda_2_joint2: {p: 2000, i: 100, d: 50}
    panda_2_joint3: {p: 2000, i: 100, d: 50}
    panda_2_joint4: {p: 2000, i: 100, d: 50}
    panda_2_joint5: {p: 2000, i: 100, d: 50}
    panda_2_joint6: {p: 2000, i: 100, d: 50}
    panda_2_joint7: {p: 2000, i: 100, d: 50}
  constraints:
    goal_time: 0.5
    stopped_velocity_tolerance: 1.0
    panda_2_joint1: { goal: 0.05 }
    panda_2_joint2: { goal: 0.05 }
    panda_2_joint3: { goal: 0.05 }
    panda_2_joint4: { goal: 0.05 }
    panda_2_joint5: { goal: 0.05 }
    panda_2_joint6: { goal: 0.05 }
    panda_2_joint7: { goal: 0.05 }

panda_1_gripper_controller:
  type: effort_controllers/GripperActionController
  joint: panda_1_finger_joint1
  gains:
    panda_1_finger_joint1: { p: 20000, i: 100, d: 50 }

panda_2_gripper_controller:
  type: effort_controllers/GripperActionController
  joint: panda_2_finger_joint1
  gains:
    panda_2_finger_joint1: { p: 20000, i: 100, d: 50 }

what's more, I connected this panda with the real panda in the lab.When I try to update the joint states using giskardpy by subscribing the specific topic,I keep get one error: /dual_panda/panda_2_joint_trajectory_controller/follow_joint_trajectory' failed to execute goal. Error: 'GOAL_TOLERANCE_VIOLATED' I am not very familar with writing yaml file, so I just copy all my yaml files. It would be great if you can give me some guidances on this issue.Building the robot on MuJoCo is not trivial at all:( I know I already asked too many questions. I am sorry that I bother you so much. I deeply appreciate your patience on my stupid question.

[HoangGiang93]

It seems like the problem is that your robot has self collision. Try to disable the collision in your environment xml (empty.xml) or set the argument disable_parent_child_collision_level in mujoco_sim.launch to -1 (meaning neglect all self collision)

[Nash-dc]

Thank you Mr.Giang, It is a self-collision problem. Thank for you help again! Finally, my thesis is almost done, I implemented a real-time digital twin using Multiverse. The following is my flowchart!

I truly appreciate your assistance with my thesis. My supervisor intends to continue using Multiverse on Windows to facilitate communication between Omniverse and MuJoCo. Thank you for your valuable contribution to this project!

However, I have a question about using MuJoCo to simulate physics. How can I interact with its environment, such as computing force or predicting an object's pose? I used to use MuJoCo-py to do the same thing, but unsure how to do with the modified MuJoCo version in C++. This isn't related to my thesis. I'm just curious about its implementation.

[HoangGiang93]

Thank you for your interest. My framework is still under development a lot and I have many plans for it. I just conducted a tutorial in IROS 2023 to introduce people about it, maybe my slide is useful for you https://seafile.zfn.uni-bremen.de/f/c4b1d47ca707438689f4/ Actually the ROS connection should be seperated from the clients. In my work there are still dependencies between MuJoCo and ROS, but I will change it to make it more modular.

[HoangGiang93]

In MuJoCo you can get all dynamics data from mjData, please look at the documentation of it, you will find a plenty of properties useful for you case

[Nash-dc]

Hi Mr.Giang, thanks for your slides, I already added part of it in my report!!

Just one small question, After trying to connect my digital twin to the real robot, I found there is often self collision between joints though I already set disable_parent_child_collision_levelto -1. Screencast from 10.10.2023 16:39:28.webm As you can see, I set joint states with the value from the panda in reality, then the behavior of the robot become very weird. It seems like the problem is that the robot still has self collision. Is there anyway can avoid that? A brief guidance would be appreciated!!

[HoangGiang93]

try to disable contact in https://mujoco.readthedocs.io/en/stable/XMLreference.html#option-flag to see if the controller is still working

[Nash-dc]

it works after disabling this! A BIG THANK YOU FOR THAT !