ceti_double.urdf.xacro

<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
  <xacro:arg name="arm_id_1" default="panda_1" />
  <xacro:arg name="arm_id_2" default="panda_2" />



  <xacro:include filename="panda_arm.xacro"/>
  <xacro:include filename="hand.xacro"/>

  <link name="world" />


  <xacro:property name="yaml_file" value="$(find mtc)/results/dummy.yaml" />
  <xacro:property name="props" value="${xacro.load_yaml(yaml_file)}" />

  <xacro:property name="x1" value="${props['objects'][8]['pos']['x']}" />
  <xacro:property name="y1" value="${props['objects'][8]['pos']['y']}" />
  <xacro:property name="z1" value="${props['objects'][8]['pos']['z']}" />
  <xacro:property name="rpy1" value="${props['objects'][8]['rpy']['r']} ${props['objects'][8]['rpy']['p']} ${props['objects'][8]['rpy']['y']}" />

  <xacro:property name="x2" value="${props['objects'][17]['pos']['x']}" />
  <xacro:property name="y2" value="${props['objects'][17]['pos']['y']}" />
  <xacro:property name="z2" value="${props['objects'][17]['pos']['z']}" />
  <xacro:property name="rpy2" value="${props['objects'][17]['rpy']['r']} ${props['objects'][17]['rpy']['p']} ${props['objects'][17]['rpy']['y']}" />


  <!-- Dummy Link -->
  <joint name="base_joint1" type="fixed">
    <parent link="world" />
    <child link="base_1" />
    <origin xyz="${x1} ${y1} ${z1 * 0.5}" rpy="${rpy1}"/>
    <axis xyz="0 0 1"/>
  </joint>

  <joint name="base_joint2" type="fixed">
    <parent link="world" />
    <child link="base_2" />
    <origin xyz="${x2} ${y2} ${z2 * 0.5}" rpy="${rpy2}"/>
    <axis xyz="0 0 1"/>
  </joint>

  
  <link name="base_1">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.8 0.8 ${z1}" />
      </geometry>
      <material name="White">
        <color rgba="1.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.8 0.8 ${z1}" />
      </geometry>
    </collision>
  </link>

  <link name="base_2">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.8 0.8 ${z2}" />
      </geometry>
      <material name="White">
        <color rgba="1.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.8 0.8 ${z2}" />
      </geometry>
    </collision>
  </link>
  
  
  <!-- right arm with gripper -->
  <xacro:panda_arm arm_id="$(arg arm_id_1)" connected_to="base_1"  xyz="-0.22 0 ${z1 * 0.5}" /> 
  <xacro:hand ns="$(arg arm_id_1)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_1)_link8" />

  <!-- left arm with gripper -->
  <xacro:panda_arm arm_id="$(arg arm_id_2)" connected_to="base_2"  xyz="-0.22 0 ${z2 * 0.5}" /> 
  <xacro:hand ns="$(arg arm_id_2)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_2)_link8"/>

</robot>