improved project structure and exposed methods
This commit is contained in:
ottofabian
@@ -0,0 +1 @@
|
||||
|
||||
@@ -0,0 +1,12 @@
|
||||
<mujocoinclude>
|
||||
<actuator boastype="none">
|
||||
<motor name="wam/shoulder_yaw_link_right_motor" joint="wam/base_yaw_joint_right"/>
|
||||
<motor name="wam/shoulder_pitch_joint_right_motor" joint='wam/shoulder_pitch_joint_right'/>
|
||||
<motor name="wam/shoulder_yaw_joint_right_motor" joint='wam/shoulder_yaw_joint_right'/>
|
||||
<motor name="wam/elbow_pitch_joint_right_motor" joint='wam/elbow_pitch_joint_right'/>
|
||||
<motor name="wam/wrist_yaw_joint_right_motor" joint='wam/wrist_yaw_joint_right'/>
|
||||
<motor name="wam/wrist_pitch_joint_right_motor" joint='wam/wrist_pitch_joint_right'/>
|
||||
<motor name="wam/palm_yaw_joint_right_motor" joint='wam/palm_yaw_joint_right'/>
|
||||
</actuator>
|
||||
</mujocoinclude>
|
||||
|
||||
@@ -0,0 +1,76 @@
|
||||
<mujocoinclude>
|
||||
<body name="wam/base_link_left" pos="-2.5 0 2" quat="0 1 0 0" childclass="wam">
|
||||
<inertial pos="0 0 0" mass="1" diaginertia="0.1 0.1 0.1"/>
|
||||
<geom class="viz" mesh="base_link_fine" rgba="0.5 0.5 0.5 0"/>
|
||||
<geom class="col" mesh="base_link_convex" rgba="0.5 0.5 0.5 1"/>
|
||||
<body name="wam/shoulder_yaw_link" pos="0 0 0.346">
|
||||
<inertial pos="-0.00443422 -0.00066489 -0.128904" quat="0.69566 0.716713 -0.0354863 0.0334839" mass="5"
|
||||
diaginertia="0.135089 0.113095 0.0904426"/>
|
||||
<joint name="wam/base_yaw_joint" range="-2.6 2.6" damping="1.98"/>
|
||||
<geom class="viz" mesh="shoulder_link_fine" rgba="1 1 1 0"/>
|
||||
<geom class="col" mesh="shoulder_link_convex_decomposition_p1"/>
|
||||
<geom class="col" mesh="shoulder_link_convex_decomposition_p2"/>
|
||||
<geom class="col" mesh="shoulder_link_convex_decomposition_p3"/>
|
||||
<body name="wam/shoulder_pitch_link" pos="0 0 0" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.00236981 -0.0154211 0.0310561" quat="0.961794 0.273112 -0.0169316 0.00866592"
|
||||
mass="3.87494" diaginertia="0.0214195 0.0167127 0.0126452"/> <!--seems off-->
|
||||
<joint name="wam/shoulder_pitch_joint" range="-1.985 1.985" damping="0.55"/>
|
||||
<geom class="viz" mesh="shoulder_pitch_link_fine" rgba="1 1 1 0"/>
|
||||
<geom class="col" mesh="shoulder_pitch_link_convex"/>
|
||||
<body name="wam/upper_arm_link" pos="0 0 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="0.00683259 3.309e-005 0.392492" quat="0.647136 0.0170822 0.0143038 0.762049"
|
||||
mass="2.20228" diaginertia="0.0592718 0.0592207 0.00313419"/>
|
||||
<joint name="wam/shoulder_yaw_joint" range="-2.8 2.8" damping="1.65"/>
|
||||
<geom class="viz" mesh="upper_arm_link_fine" rgba="1 1 1 0"/>
|
||||
<geom class="col" mesh="upper_arm_link_convex_decomposition_p1" rgba="0.094 0.48 0.804 1"/>
|
||||
<geom class="col" mesh="upper_arm_link_convex_decomposition_p2" rgba="0.094 0.48 0.804 1"/>
|
||||
<body name="wam/forearm_link" pos="0.045 0 0.55" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.0400149 -0.142717 -0.00022942"
|
||||
quat="0.704281 0.706326 0.0180333 0.0690353" mass="0.500168"
|
||||
diaginertia="0.0151047 0.0148285 0.00275805"/>
|
||||
<joint name="wam/elbow_pitch_joint" range="-0.9 3.14159" damping="0.88"/>
|
||||
<geom class="viz" mesh="elbow_link_fine" rgba="1 1 1 0"/>
|
||||
<geom class="col" mesh="elbow_link_convex"/>
|
||||
<geom class="viz" mesh="forearm_link_fine" pos="-.045 -0.0730 0" euler="1.57 0 0" rgba="1 1 1 0"/>
|
||||
<geom class="col" mesh="forearm_link_convex_decomposition_p1" pos="-0.045 -0.0730 0"
|
||||
euler="1.57 0 0" rgba="0.094 0.48 0.804 1"/>
|
||||
<geom class="col" mesh="forearm_link_convex_decomposition_p2" pos="-.045 -0.0730 0"
|
||||
euler="1.57 0 0" rgba="0.094 0.48 0.804 1"/>
|
||||
<body name="wam/wrist_yaw_link" pos="-0.045 -0.3 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="8.921e-005 0.00435824 -0.00511217"
|
||||
quat="0.630602 0.776093 0.00401969 -0.002372" mass="1.05376"
|
||||
diaginertia="0.000555168 0.00046317 0.000234072"/> <!--this is an approximation-->
|
||||
<joint name="wam/wrist_yaw_joint" range="-4.55 1.25" damping="0.55"/>
|
||||
<geom class="viz" mesh="wrist_yaw_link_fine" rgba="1 1 1 0"/>
|
||||
<geom class="col" mesh="wrist_yaw_link_convex_decomposition_p1"/>
|
||||
<geom class="col" mesh="wrist_yaw_link_convex_decomposition_p2"/>
|
||||
<body name="wam/wrist_pitch_link" pos="0 0 0" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.00012262 -0.0246834 -0.0170319"
|
||||
quat="0.630602 0.776093 0.00401969 -0.002372" mass="0.517974"
|
||||
diaginertia="0.000555168 0.00046317 0.000234072"/>
|
||||
<joint name="wam/wrist_pitch_joint" range="-1.5707 1.5707" damping="0.11"/>
|
||||
<geom class="viz" mesh="wrist_pitch_link_fine" rgba="1 1 1 0"/>
|
||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p1" rgba="1 0.5 0.313 1"/>
|
||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p2" rgba="1 0.5 0.313 1"/>
|
||||
<geom class="col" mesh="wrist_pitch_link_convex_decomposition_p3" rgba="1 0.5 0.313 1"/>
|
||||
<body name="wam/wrist_palm_link" pos="0 0 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="0 0 0.055" quat="0.707107 0 0 0.707107" mass="0.0828613"
|
||||
diaginertia="0.00020683 0.00010859 0.00010851"/>
|
||||
<joint name="wam/palm_yaw_joint" range="-3 3" damping="0.11"/>
|
||||
<geom class="viz" mesh="wrist_palm_link_fine" rgba="1 1 1 0"/>
|
||||
<geom class="col" mesh="wrist_palm_link_convex"/>
|
||||
<body name="paddle_left" pos="0 0 0.26" childclass="contact_geom">
|
||||
<geom name="bat_left" type="cylinder" size="0.075 0.0015" rgba="1 0 0 1"
|
||||
quat="0.71 0 0.71 0"/>
|
||||
<geom name="handle_left" type="box" size="0.005 0.01 0.05" pos="0 0 -0.08"
|
||||
rgba="1 1 1 1"/>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</mujocoinclude>
|
||||
@@ -0,0 +1,95 @@
|
||||
<mujocoinclue>
|
||||
<body name="wam/base_link_right" pos="2.5 0 2" quat="0 0 1 0" childclass="wam" >
|
||||
<inertial pos="0 0 0" mass="1" diaginertia="0.1 0.1 0.1"/>
|
||||
<geom name="base_link_fine" class="viz" mesh="base_link_fine" rgba="0.5 0.5 0.5 0"/>
|
||||
<geom name="base_link_convex" class="col" mesh="base_link_convex" rgba="0.5 0.5 0.5 1"/>
|
||||
<body name="wam/shoulder_yaw_link_right" pos="0 0 0.346">
|
||||
<inertial pos="-0.00443422 -0.00066489 -0.128904" quat="0.69566 0.716713 -0.0354863 0.0334839" mass="5"
|
||||
diaginertia="0.135089 0.113095 0.0904426"/>
|
||||
<joint name="wam/base_yaw_joint_right" range="-2.6 2.6" damping="1.98"/>
|
||||
<geom name="shoulder_link_fine" class="viz" mesh="shoulder_link_fine" rgba="1 1 1 0"/>
|
||||
<geom name="shoulder_link_convex_decomposition_p1" class="col"
|
||||
mesh="shoulder_link_convex_decomposition_p1"/>
|
||||
<geom name="shoulder_link_convex_decomposition_p2" class="col"
|
||||
mesh="shoulder_link_convex_decomposition_p2"/>
|
||||
<geom name="shoulder_link_convex_decomposition_p3" class="col"
|
||||
mesh="shoulder_link_convex_decomposition_p3"/>
|
||||
<body name="wam/shoulder_pitch_link_right" pos="0 0 0" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.00236981 -0.0154211 0.0310561" quat="0.961794 0.273112 -0.0169316 0.00866592"
|
||||
mass="3.87494" diaginertia="0.0214195 0.0167127 0.0126452"/> <!--seems off-->
|
||||
<joint name="wam/shoulder_pitch_joint_right" range="-2 2" damping="0.55"/>
|
||||
<geom name="shoulder_pitch_link_fine" class="viz" mesh="shoulder_pitch_link_fine" rgba="1 1 1 0"/>
|
||||
<geom name="shoulder_pitch_link_convex" class="col" mesh="shoulder_pitch_link_convex"/>
|
||||
<body name="wam/upper_arm_link_right" pos="0 0 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="0.00683259 3.309e-005 0.392492" quat="0.647136 0.0170822 0.0143038 0.762049"
|
||||
mass="2.20228" diaginertia="0.0592718 0.0592207 0.00313419"/>
|
||||
<joint name="wam/shoulder_yaw_joint_right" range="-2.8 2.8" damping="1.65"/>
|
||||
<geom name="upper_arm_link_fine" class="viz" mesh="upper_arm_link_fine" rgba="1 1 1 0"/>
|
||||
<geom name="upper_arm_link_convex_decomposition_p1" class="col"
|
||||
mesh="upper_arm_link_convex_decomposition_p1" rgba="0.094 0.48 0.804 1"/>
|
||||
<geom name="upper_arm_link_convex_decomposition_p2" class="col"
|
||||
mesh="upper_arm_link_convex_decomposition_p2" rgba="0.094 0.48 0.804 1"/>
|
||||
<body name="wam/forearm_link_right" pos="0.045 0 0.55" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.0400149 -0.142717 -0.00022942"
|
||||
quat="0.704281 0.706326 0.0180333 0.0690353" mass="0.500168"
|
||||
diaginertia="0.0151047 0.0148285 0.00275805"/>
|
||||
<joint name="wam/elbow_pitch_joint_right" range="-0.9 3.1" damping="0.88"/>
|
||||
<geom name="elbow_link_fine" class="viz" mesh="elbow_link_fine" rgba="1 1 1 0"/>
|
||||
<geom name="elbow_link_convex" class="col" mesh="elbow_link_convex"/>
|
||||
<geom name="forearm_link_fine" class="viz" mesh="forearm_link_fine" pos="-.045 -0.0730 0"
|
||||
euler="1.57 0 0" rgba="1 1 1 0"/>
|
||||
<geom name="forearm_link_convex_decomposition_p1" class="col"
|
||||
mesh="forearm_link_convex_decomposition_p1" pos="-0.045 -0.0730 0"
|
||||
euler="1.57 0 0" rgba="0.094 0.48 0.804 1"/>
|
||||
<geom name="forearm_link_convex_decomposition_p2" class="col"
|
||||
mesh="forearm_link_convex_decomposition_p2" pos="-.045 -0.0730 0"
|
||||
euler="1.57 0 0" rgba="0.094 0.48 0.804 1"/>
|
||||
<body name="wam/wrist_yaw_link_right" pos="-0.045 -0.3 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="8.921e-005 0.00435824 -0.00511217"
|
||||
quat="0.630602 0.776093 0.00401969 -0.002372" mass="1.05376"
|
||||
diaginertia="0.000555168 0.00046317 0.000234072"/> <!--this is an approximation-->
|
||||
<joint name="wam/wrist_yaw_joint_right" range="-4.8 1.3" damping="0.55"/>
|
||||
<geom name="wrist_yaw_link_fine" class="viz" mesh="wrist_yaw_link_fine" rgba="1 1 1 0"/>
|
||||
<geom name="wrist_yaw_link_convex_decomposition_p1" class="col"
|
||||
mesh="wrist_yaw_link_convex_decomposition_p1"/>
|
||||
<geom name="wrist_yaw_link_convex_decomposition_p2" class="col"
|
||||
mesh="wrist_yaw_link_convex_decomposition_p2"/>
|
||||
<body name="wam/wrist_pitch_link_right" pos="0 0 0" quat="0.707107 -0.707107 0 0">
|
||||
<inertial pos="-0.00012262 -0.0246834 -0.0170319"
|
||||
quat="0.630602 0.776093 0.00401969 -0.002372" mass="0.517974"
|
||||
diaginertia="0.000555168 0.00046317 0.000234072"/>
|
||||
<joint name="wam/wrist_pitch_joint_right" range="-1.6 1.6" damping="0.11"/>
|
||||
<geom name="wrist_pitch_link_fine" class="viz" mesh="wrist_pitch_link_fine"
|
||||
rgba="1 1 1 0"/>
|
||||
<geom name="wrist_pitch_link_convex_decomposition_p1" rgba="1 0.5 0.313 1"
|
||||
class="col" mesh="wrist_pitch_link_convex_decomposition_p1"/>
|
||||
<geom name="wrist_pitch_link_convex_decomposition_p2" rgba="1 0.5 0.313 1"
|
||||
class="col" mesh="wrist_pitch_link_convex_decomposition_p2"/>
|
||||
<geom name="wrist_pitch_link_convex_decomposition_p3" rgba="1 0.5 0.313 1"
|
||||
class="col" mesh="wrist_pitch_link_convex_decomposition_p3"/>
|
||||
<body name="wam/wrist_palm_link_right" pos="0 0 0" quat="0.707107 0.707107 0 0">
|
||||
<inertial pos="0 0 0.055" quat="0.707107 0 0 0.707107" mass="0.0828613"
|
||||
diaginertia="0.00020683 0.00010859 0.00010851"/>
|
||||
<joint name="wam/palm_yaw_joint_right" range="-2.2 2.2" damping="0.11"/>
|
||||
<geom name="wrist_palm_link_fine" class="viz" mesh="wrist_palm_link_fine"
|
||||
rgba="1 1 1 0"/>
|
||||
<geom name="wrist_palm_link_convex" class="col" mesh="wrist_palm_link_convex"/>
|
||||
<!-- EE=wam/paddle, configure name to the end effector name-->
|
||||
<body name="EE" pos="0 0 0.26" childclass="contact_geom">
|
||||
<geom name="bat" type="cylinder" size="0.075 0.005" rgba="1 0 0 1"
|
||||
quat="0.71 0 0.71 0"/>
|
||||
<geom name="wam/paddle_handle" type="box" size="0.005 0.01 0.05" pos="0 0 -0.08"
|
||||
rgba="1 1 1 1"/>
|
||||
<!-- Extract information for sampling goals.-->
|
||||
<site name="wam/paddle_center" pos="0 0 0" rgba="1 1 1 1" size="0.00001"/>
|
||||
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</body>
|
||||
</mujocoinclue>
|
||||
@@ -0,0 +1,38 @@
|
||||
<mujocoinclude>
|
||||
<body name="table_tennis_table" pos="0 0 0">
|
||||
<geom class="contact_geom" name="table_base_1" type="box" size="0.05 0.05 .375" rgba="1 1 1 1"
|
||||
pos="1 0.7 0.375"/>
|
||||
<geom class="contact_geom" name="table_base_2" type="box" size="0.05 0.05 .375" rgba="1 1 1 1"
|
||||
pos="1 -0.7 0.375"/>
|
||||
<geom class="contact_geom" name="table_base_3" type="box" size="0.05 0.05 .375" rgba="1 1 1 1"
|
||||
pos="-1 -0.7 0.375"/>
|
||||
<geom class="contact_geom" name="table_base_4" type="box" size="0.05 0.05 .375" rgba="1 1 1 1"
|
||||
pos="-1 0.7 0.375"/>
|
||||
<body name="table_top" pos="0 0 0.76">
|
||||
<geom class="contact_geom" name="table_tennis_table" type="box" size="1.37 .7625 .01" rgba="0 0 0.5 1"
|
||||
pos="0 0 0"/>
|
||||
<!-- <geom class="contact_geom" name="table_tennis_table_right_side" type="box" size="0.685 .7625 .01"-->
|
||||
<!-- rgba="0.5 0 0 1" pos="0.685 0 0"/>-->
|
||||
<!-- <geom class="contact_geom" name="table_tennis_table_left_side" type="box" size="0.685 .7625 .01"-->
|
||||
<!-- rgba="0 0.5 0 1" pos="-0.685 0 0"/>-->
|
||||
<site name="left_up_corner" pos="-1.37 .7625 0.01" rgba="1 1 1 1" size="0.00001"/>
|
||||
<site name="middle_up_corner" pos="0 .7625 0.01" rgba="1 1 1 1" size="0.00001"/>
|
||||
<site name="left_down_corner" pos="-1.37 -0.7625 0.01" rgba="1 1 1 1" size="0.00001"/>
|
||||
<site name="middle_down_corner" pos="0 -.7625 0.01" rgba="1 1 1 1" size="0.00001"/>
|
||||
<geom class="contact_geom" name="table_tennis_net" type="box" size="0.01 0.915 0.07625"
|
||||
material="floor_plane"
|
||||
rgba="0 0 1 0.5"
|
||||
pos="0 0 0.08625"/>
|
||||
<geom class="contact_geom" name="left_while_line" type="box" size="1.37 .02 .001" rgba="1 1 1 1"
|
||||
pos="0 -0.7425 0.01"/>
|
||||
<geom class="contact_geom" name="center_while_line" type="box" size="1.37 .01 .001" rgba="1 1 1 1"
|
||||
pos="0 0 0.01"/>
|
||||
<geom class="contact_geom" name="right_while_line" type="box" size="1.37 .02 .001" rgba="1 1 1 1"
|
||||
pos="0 0.7425 0.01"/>
|
||||
<geom class="contact_geom" name="right_side_line" type="box" size="0.02 .7625 .001" rgba="1 1 1 1"
|
||||
pos="1.35 0 0.01"/>
|
||||
<geom class="contact_geom" name="left_side_line" type="box" size="0.02 .7625 .001" rgba="1 1 1 1"
|
||||
pos="-1.35 0 0.01"/>
|
||||
</body>
|
||||
</body>
|
||||
</mujocoinclude>
|
||||
@@ -0,0 +1,10 @@
|
||||
<mujocoinclude>
|
||||
<body name="target_ball" pos="-1.2 -0.6 1.5">
|
||||
<joint axis="1 0 0" damping="0.0" name="tar:x" pos="0 0 0" stiffness="0" type="slide" frictionloss="0"/>
|
||||
<joint axis="0 1 0" damping="0.0" name="tar:y" pos="0 0 0" stiffness="0" type="slide" frictionloss="0"/>
|
||||
<joint axis="0 0 1" damping="0.0" name="tar:z" pos="0 0 0" stiffness="0" type="slide" frictionloss="0"/>
|
||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="target_ball" rgba="1 1 0 1" mass="0.1"
|
||||
friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>
|
||||
<site name="target_ball" pos="0 0 0" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
|
||||
</body>
|
||||
</mujocoinclude>
|
||||
@@ -0,0 +1,80 @@
|
||||
<mujocoinclude>
|
||||
<body name="test_ball_table" pos="1 0 4">
|
||||
<joint axis="1 0 0" damping="0.0" name="tar:x_test_ball_table" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<joint axis="0 1 0" damping="0.0" name="tar:y_test_ball_table" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<joint axis="0 0 1" damping="0.0" name="tar:z_test_ball_table" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="test_ball_table" rgba="0 1 0 1" mass="0.1"
|
||||
friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>
|
||||
<site name="test_ball_table" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
|
||||
</body>
|
||||
<body name="test_ball_net" pos="0 0 4">
|
||||
<joint axis="1 0 0" damping="0.0" name="tar:x_test_ball_net" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<joint axis="0 1 0" damping="0.0" name="tar:y_test_ball_net" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<joint axis="0 0 1" damping="0.0" name="tar:z_test_ball_net" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="test_ball_net" rgba="1 1 0 1" mass="0.1"
|
||||
friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>
|
||||
<site name="test_ball_net" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
|
||||
</body>
|
||||
<body name="test_ball_racket_0" pos="2.54919187 0.81642672 4">
|
||||
<joint axis="1 0 0" damping="0.0" name="tar:x_test_ball_racket_0" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<joint axis="0 1 0" damping="0.0" name="tar:y_test_ball_racket_0" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<joint axis="0 0 1" damping="0.0" name="tar:z_test_ball_racket_0" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="test_ball_racket_0" rgba="1 0 1 1" mass="0.1"
|
||||
friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>
|
||||
<site name="test_ball_racket_0" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
|
||||
</body>
|
||||
<body name="test_ball_racket_1" pos="2.54919187 0.81642672 4.5">
|
||||
<joint axis="1 0 0" damping="0.0" name="tar:x_test_ball_racket_1" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<joint axis="0 1 0" damping="0.0" name="tar:y_test_ball_racket_1" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<joint axis="0 0 1" damping="0.0" name="tar:z_test_ball_racket_1" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="test_ball_racket_1" rgba="1 0 1 1" mass="0.1"
|
||||
friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>
|
||||
<site name="test_ball_racket_1" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
|
||||
</body>
|
||||
<body name="test_ball_racket_2" pos="2.54919187 0.81642672 3">
|
||||
<joint axis="1 0 0" damping="0.0" name="tar:x_test_ball_racket_2" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<joint axis="0 1 0" damping="0.0" name="tar:y_test_ball_racket_2" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<joint axis="0 0 1" damping="0.0" name="tar:z_test_ball_racket_2" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="test_ball_racket_2" rgba="1 0 1 1" mass="0.1"
|
||||
friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>
|
||||
<site name="test_ball_racket" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
|
||||
</body>
|
||||
<body name="test_ball_racket_3" pos="2.54919187 0.81642672 10">
|
||||
<joint axis="1 0 0" damping="0.0" name="tar:x_test_ball_racket_3" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<joint axis="0 1 0" damping="0.0" name="tar:y_test_ball_racket_3" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<joint axis="0 0 1" damping="0.0" name="tar:z_test_ball_racket_3" pos="0 0 0" stiffness="0" type="slide"
|
||||
frictionloss="0"/>
|
||||
<geom size="0.025 0.025 0.025" type="sphere" condim="4" name="test_ball_racket_3" rgba="1 0 1 1" mass="0.1"
|
||||
friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>
|
||||
<site name="test_ball_racket_3" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
|
||||
</body>
|
||||
<!-- <body name="test_ball_racket_4" pos="2.54919187 0.81642672 4">-->
|
||||
<!-- <joint axis="1 0 0" damping="0.0" name="tar:x_test_ball_racket_4" pos="0 0 0" stiffness="0" type="slide"-->
|
||||
<!-- frictionloss="0"/>-->
|
||||
<!-- <joint axis="0 1 0" damping="0.0" name="tar:y_test_ball_racket_4" pos="0 0 0" stiffness="0" type="slide"-->
|
||||
<!-- frictionloss="0"/>-->
|
||||
<!-- <joint axis="0 0 1" damping="0.0" name="tar:z_test_ball_racket_4" pos="0 0 0" stiffness="0" type="slide"-->
|
||||
<!-- frictionloss="0"/>-->
|
||||
<!-- <geom size="0.025 0.025 0.025" type="sphere" condim="4" name="test_ball_racket_4" rgba="1 0 0 1" mass="0.1"-->
|
||||
<!-- friction="0.1 0.1 0.1" solimp="1 1 0" solref="0.1 0.03"/>-->
|
||||
<!-- <site name="test_ball_racket_4" pos="0 0 0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>-->
|
||||
<!-- </body>-->
|
||||
|
||||
</mujocoinclude>
|
||||
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,19 @@
|
||||
<mujocoinclude>
|
||||
<actuator>
|
||||
|
||||
<!-- <position ctrlrange="-2.6 2.6" joint="wam/base_yaw_joint_right" kp="100.0" />-->
|
||||
<!-- <position ctrlrange="-1.985 1.985" joint="wam/shoulder_pitch_joint_right" kp="162.0" />-->
|
||||
<!-- <position ctrlrange="-2.8 2.8" joint="wam/shoulder_yaw_joint_right" kp="100.0" />-->
|
||||
<!-- <position ctrlrange="-0.9 3.14159" joint="wam/elbow_pitch_joint_right" kp="122.0" />-->
|
||||
<!-- <position ctrlrange="-4.55 1.25" joint="wam/wrist_yaw_joint_right" kp="100.0" />-->
|
||||
<!-- <position ctrlrange="-1.5707 1.5707" joint="wam/wrist_pitch_joint_right" kp="102.0" />-->
|
||||
<!-- <position ctrlrange="-3 3" joint="wam/palm_yaw_joint_right" kp="100.0" />-->
|
||||
<position ctrlrange="-2.6 2.6" joint="wam/base_yaw_joint_right" kp="151.0"/>
|
||||
<position ctrlrange="-1.985 1.985" joint="wam/shoulder_pitch_joint_right" kp="125.0"/>
|
||||
<position ctrlrange="-2.8 2.8" joint="wam/shoulder_yaw_joint_right" kp="122.0"/>
|
||||
<position ctrlrange="-0.9 3.14159" joint="wam/elbow_pitch_joint_right" kp="121.0"/>
|
||||
<position ctrlrange="-4.55 1.25" joint="wam/wrist_yaw_joint_right" kp="99.0"/>
|
||||
<position ctrlrange="-1.5707 1.5707" joint="wam/wrist_pitch_joint_right" kp="103.0"/>
|
||||
<position ctrlrange="-3 3" joint="wam/palm_yaw_joint_right" kp="99.0"/>
|
||||
</actuator>
|
||||
</mujocoinclude>
|
||||
@@ -0,0 +1,49 @@
|
||||
<mujocoinclude>
|
||||
<default>
|
||||
<default class="wam">
|
||||
<joint type="hinge" limited="true" pos="0 0 0" axis="0 0 1"/>
|
||||
</default>
|
||||
<default class="viz">
|
||||
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="1 1 1 1"/>
|
||||
</default>
|
||||
<default class="col">
|
||||
<geom type="mesh" contype="0" conaffinity="1" group="0" rgba="1 1 1 1"/>
|
||||
</default>
|
||||
<default class="contact_geom">
|
||||
<geom condim="4" friction="0.1 0.1 0.1" margin="0" solimp="1 1 0" solref="0.1 0.03"/>
|
||||
<!-- <geom condim="4" friction="0 0 0" margin="0" solimp="1 1 0" solref="0.01 1.1"/>-->
|
||||
|
||||
</default>
|
||||
</default>
|
||||
<asset>
|
||||
<mesh file="base_link_fine.stl"/>
|
||||
<mesh file="base_link_convex.stl"/>
|
||||
<mesh file="shoulder_link_fine.stl"/>
|
||||
<mesh file="shoulder_link_convex_decomposition_p1.stl"/>
|
||||
<mesh file="shoulder_link_convex_decomposition_p2.stl"/>
|
||||
<mesh file="shoulder_link_convex_decomposition_p3.stl"/>
|
||||
<mesh file="shoulder_pitch_link_fine.stl"/>
|
||||
<mesh file="shoulder_pitch_link_convex.stl"/>
|
||||
<mesh file="upper_arm_link_fine.stl"/>
|
||||
<mesh file="upper_arm_link_convex_decomposition_p1.stl"/>
|
||||
<mesh file="upper_arm_link_convex_decomposition_p2.stl"/>
|
||||
<mesh file="elbow_link_fine.stl"/>
|
||||
<mesh file="elbow_link_convex.stl"/>
|
||||
<mesh file="forearm_link_fine.stl"/>
|
||||
<mesh file="forearm_link_convex_decomposition_p1.stl"/>
|
||||
<mesh file="forearm_link_convex_decomposition_p2.stl"/>
|
||||
<mesh file="wrist_yaw_link_fine.stl"/>
|
||||
<mesh file="wrist_yaw_link_convex_decomposition_p1.stl"/>
|
||||
<mesh file="wrist_yaw_link_convex_decomposition_p2.stl"/>
|
||||
<mesh file="wrist_pitch_link_fine.stl"/>
|
||||
<mesh file="wrist_pitch_link_convex_decomposition_p1.stl"/>
|
||||
<mesh file="wrist_pitch_link_convex_decomposition_p2.stl"/>
|
||||
<mesh file="wrist_pitch_link_convex_decomposition_p3.stl"/>
|
||||
<mesh file="wrist_palm_link_fine.stl"/>
|
||||
<mesh file="wrist_palm_link_convex.stl"/>
|
||||
|
||||
<texture builtin="checker" height="512" name="texplane" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2" type="2d"
|
||||
width="512"/>
|
||||
<material name="floor_plane" reflectance="0.5" texrepeat="1 1" texture="texplane" texuniform="true"/>
|
||||
</asset>
|
||||
</mujocoinclude>
|
||||
@@ -0,0 +1,41 @@
|
||||
<mujoco model="table_tennis(v0.1)">
|
||||
<compiler angle="radian" coordinate="local" meshdir="meshes/" />
|
||||
|
||||
<option gravity="0 0 -9.81" timestep="0.002">
|
||||
<flag warmstart="enable" />
|
||||
</option>
|
||||
|
||||
|
||||
<custom>
|
||||
<numeric data="0 0 0 0 0 0 0" name="START_ANGLES" />
|
||||
</custom>
|
||||
|
||||
|
||||
<include file="shared.xml" />
|
||||
|
||||
<worldbody>
|
||||
<light cutoff="60" diffuse="1 1 1" dir="-.1 -.2 -1.3" directional="true" exponent="1" pos=".1 .2 1.3" specular=".1 .1 .1" />
|
||||
<geom conaffinity="1" contype="1" material="floor_plane" name="floor" pos="0 0 0" size="10 5 1" type="plane" />
|
||||
|
||||
|
||||
<include file="include_table.xml" />
|
||||
|
||||
|
||||
|
||||
<include file="include_barrett_wam_7dof_right.xml" />
|
||||
|
||||
<include file="include_target_ball.xml" />
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
</worldbody>
|
||||
|
||||
|
||||
|
||||
|
||||
<include file="right_arm_actuator.xml" />
|
||||
|
||||
|
||||
</mujoco>
|
||||
@@ -0,0 +1,244 @@
|
||||
import numpy as np
|
||||
from gym import spaces
|
||||
from gym.envs.robotics import robot_env, utils
|
||||
# import xml.etree.ElementTree as ET
|
||||
from alr_envs.alr.mujoco.gym_table_tennis.utils.rewards.hierarchical_reward import HierarchicalRewardTableTennis
|
||||
import glfw
|
||||
from alr_envs.alr.mujoco.gym_table_tennis.utils.experiment import ball_initialize
|
||||
from pathlib import Path
|
||||
import os
|
||||
|
||||
|
||||
class TableTennisEnv(robot_env.RobotEnv):
|
||||
"""Class for Table Tennis environment.
|
||||
"""
|
||||
def __init__(self, n_substeps=1,
|
||||
model_path=None,
|
||||
initial_qpos=None,
|
||||
initial_ball_state=None,
|
||||
config=None,
|
||||
reward_obj=None
|
||||
):
|
||||
"""Initializes a new mujoco based Table Tennis environment.
|
||||
|
||||
Args:
|
||||
model_path (string): path to the environments XML file
|
||||
initial_qpos (dict): a dictionary of joint names and values that define the initial
|
||||
n_actions: Number of joints
|
||||
n_substeps (int): number of substeps the simulation runs on every call to step
|
||||
scale (double): limit maximum change in position
|
||||
initial_ball_state: to reset the ball state
|
||||
"""
|
||||
# self.config = config.config
|
||||
if model_path is None:
|
||||
path_cws = Path.cwd()
|
||||
print(path_cws)
|
||||
current_dir = Path(os.path.split(os.path.realpath(__file__))[0])
|
||||
table_tennis_env_xml_path = current_dir / "assets"/"table_tennis_env.xml"
|
||||
model_path = str(table_tennis_env_xml_path)
|
||||
self.config = config
|
||||
action_space = True # self.config['trajectory']['args']['action_space']
|
||||
time_step = 0.002 # self.config['mujoco_sim_env']['args']["time_step"]
|
||||
if initial_qpos is None:
|
||||
initial_qpos = {"wam/base_yaw_joint_right": 1.5,
|
||||
"wam/shoulder_pitch_joint_right": 1,
|
||||
"wam/shoulder_yaw_joint_right": 0,
|
||||
"wam/elbow_pitch_joint_right": 1,
|
||||
"wam/wrist_yaw_joint_right": 1,
|
||||
"wam/wrist_pitch_joint_right": 0,
|
||||
"wam/palm_yaw_joint_right": 0}
|
||||
# initial_qpos = [1.5, 1, 0, 1, 1, 0, 0] # self.config['robot_config']['args']['initial_qpos']
|
||||
|
||||
# TODO should read all configuration in config
|
||||
assert initial_qpos is not None, "Must initialize the initial q position of robot arm"
|
||||
n_actions = 7
|
||||
self.initial_qpos_value = np.array(list(initial_qpos.values())).copy()
|
||||
# self.initial_qpos_value = np.array(initial_qpos)
|
||||
# # change time step in .xml file
|
||||
# tree = ET.parse(model_path)
|
||||
# root = tree.getroot()
|
||||
# for option in root.findall('option'):
|
||||
# option.set("timestep", str(time_step))
|
||||
#
|
||||
# tree.write(model_path)
|
||||
|
||||
super(TableTennisEnv, self).__init__(
|
||||
model_path=model_path, n_substeps=n_substeps, n_actions=n_actions,
|
||||
initial_qpos=initial_qpos)
|
||||
|
||||
if action_space:
|
||||
self.action_space = spaces.Box(low=np.array([-2.6, -2.0, -2.8, -0.9, -4.8, -1.6, -2.2]),
|
||||
high=np.array([2.6, 2.0, 2.8, 3.1, 1.3, 1.6, 2.2]),
|
||||
dtype='float64')
|
||||
else:
|
||||
self.action_space = spaces.Box(low=np.array([-np.inf] * 7),
|
||||
high=np.array([-np.inf] * 7),
|
||||
dtype='float64')
|
||||
self.scale = None
|
||||
self.desired_pos = None
|
||||
self.n_actions = n_actions
|
||||
self.action = None
|
||||
self.time_step = time_step
|
||||
self._dt = time_step
|
||||
self.paddle_center_pos = self.sim.data.get_site_xpos('wam/paddle_center')
|
||||
if reward_obj is None:
|
||||
self.reward_obj = HierarchicalRewardTableTennis()
|
||||
else:
|
||||
self.reward_obj = reward_obj
|
||||
|
||||
if initial_ball_state is not None:
|
||||
self.initial_ball_state = initial_ball_state
|
||||
else:
|
||||
self.initial_ball_state = ball_initialize(random=False)
|
||||
self.target_ball_pos = self.sim.data.get_site_xpos("target_ball")
|
||||
self.racket_center_pos = self.sim.data.get_site_xpos("wam/paddle_center")
|
||||
|
||||
def close(self):
|
||||
if self.viewer is not None:
|
||||
glfw.destroy_window(self.viewer.window)
|
||||
# self.viewer.window.close()
|
||||
self.viewer = None
|
||||
self._viewers = {}
|
||||
|
||||
# GoalEnv methods
|
||||
# ----------------------------
|
||||
def compute_reward(self, achieved_goal, goal, info):
|
||||
# reset the reward, if action valid
|
||||
# right_court_contact_obj = ["target_ball", "table_tennis_table_right_side"]
|
||||
# right_court_contact_detector = self.reward_obj.contact_detection(self, right_court_contact_obj)
|
||||
# if right_court_contact_detector:
|
||||
# print("can detect the table ball contact")
|
||||
self.reward_obj.total_reward = 0
|
||||
# Stage 1 Hitting
|
||||
self.reward_obj.hitting(self)
|
||||
# if not hitted, return the highest reward
|
||||
if not self.reward_obj.goal_achievement:
|
||||
# return self.reward_obj.highest_reward
|
||||
return self.reward_obj.total_reward
|
||||
# # Stage 2 Right Table Contact
|
||||
# self.reward_obj.right_table_contact(self)
|
||||
# if not self.reward_obj.goal_achievement:
|
||||
# return self.reward_obj.highest_reward
|
||||
# # Stage 2 Net Contact
|
||||
# self.reward_obj.net_contact(self)
|
||||
# if not self.reward_obj.goal_achievement:
|
||||
# return self.reward_obj.highest_reward
|
||||
# Stage 3 Opponent court Contact
|
||||
# self.reward_obj.landing_on_opponent_court(self)
|
||||
# if not self.reward_obj.goal_achievement:
|
||||
# print("self.reward_obj.highest_reward: ", self.reward_obj.highest_reward)
|
||||
# TODO
|
||||
self.reward_obj.target_achievement(self)
|
||||
# return self.reward_obj.highest_reward
|
||||
return self.reward_obj.total_reward
|
||||
|
||||
def _reset_sim(self):
|
||||
self.sim.set_state(self.initial_state)
|
||||
[initial_x, initial_y, initial_z, v_x, v_y, v_z] = self.initial_ball_state
|
||||
self.sim.data.set_joint_qpos('tar:x', initial_x)
|
||||
self.sim.data.set_joint_qpos('tar:y', initial_y)
|
||||
self.sim.data.set_joint_qpos('tar:z', initial_z)
|
||||
self.energy_corrected = True
|
||||
self.give_reflection_reward = False
|
||||
|
||||
# velocity is positive direction
|
||||
self.sim.data.set_joint_qvel('tar:x', v_x)
|
||||
self.sim.data.set_joint_qvel('tar:y', v_y)
|
||||
self.sim.data.set_joint_qvel('tar:z', v_z)
|
||||
|
||||
# Apply gravity compensation
|
||||
if self.sim.data.qfrc_applied[:7] is not self.sim.data.qfrc_bias[:7]:
|
||||
self.sim.data.qfrc_applied[:7] = self.sim.data.qfrc_bias[:7]
|
||||
self.sim.forward()
|
||||
return True
|
||||
|
||||
def _env_setup(self, initial_qpos):
|
||||
for name, value in initial_qpos.items():
|
||||
self.sim.data.set_joint_qpos(name, value)
|
||||
|
||||
# Apply gravity compensation
|
||||
if self.sim.data.qfrc_applied[:7] is not self.sim.data.qfrc_bias[:7]:
|
||||
self.sim.data.qfrc_applied[:7] = self.sim.data.qfrc_bias[:7]
|
||||
self.sim.forward()
|
||||
|
||||
# Get the target position
|
||||
self.initial_paddle_center_xpos = self.sim.data.get_site_xpos('wam/paddle_center').copy()
|
||||
self.initial_paddle_center_vel = None # self.sim.get_site_
|
||||
|
||||
def _sample_goal(self):
|
||||
goal = self.initial_paddle_center_xpos[:3] + self.np_random.uniform(-0.2, 0.2, size=3)
|
||||
return goal.copy()
|
||||
|
||||
def _get_obs(self):
|
||||
|
||||
# positions of racket center
|
||||
paddle_center_pos = self.sim.data.get_site_xpos('wam/paddle_center')
|
||||
ball_pos = self.sim.data.get_site_xpos("target_ball")
|
||||
|
||||
dt = self.sim.nsubsteps * self.sim.model.opt.timestep
|
||||
paddle_center_velp = self.sim.data.get_site_xvelp('wam/paddle_center') * dt
|
||||
robot_qpos, robot_qvel = utils.robot_get_obs(self.sim)
|
||||
|
||||
wrist_state = robot_qpos[-3:]
|
||||
wrist_vel = robot_qvel[-3:] * dt # change to a scalar if the gripper is made symmetric
|
||||
|
||||
# achieved_goal = paddle_body_EE_pos
|
||||
obs = np.concatenate([
|
||||
paddle_center_pos, paddle_center_velp, wrist_state, wrist_vel
|
||||
])
|
||||
|
||||
out_dict = {
|
||||
'observation': obs.copy(),
|
||||
'achieved_goal': paddle_center_pos.copy(),
|
||||
'desired_goal': self.goal.copy(),
|
||||
'q_pos': self.sim.data.qpos[:].copy(),
|
||||
"ball_pos": ball_pos.copy(),
|
||||
# "hitting_flag": self.reward_obj.hitting_flag
|
||||
}
|
||||
|
||||
return out_dict
|
||||
|
||||
def _step_callback(self):
|
||||
pass
|
||||
|
||||
def _set_action(self, action):
|
||||
# Apply gravity compensation
|
||||
if self.sim.data.qfrc_applied[:7] is not self.sim.data.qfrc_bias[:7]:
|
||||
self.sim.data.qfrc_applied[:7] = self.sim.data.qfrc_bias[:7]
|
||||
# print("set action process running")
|
||||
assert action.shape == (self.n_actions,)
|
||||
self.action = action.copy() # ensure that we don't change the action outside of this scope
|
||||
pos_ctrl = self.action[:] # limit maximum change in position
|
||||
pos_ctrl = np.clip(pos_ctrl, self.action_space.low, self.action_space.high)
|
||||
|
||||
# get desired trajectory
|
||||
self.sim.data.qpos[:7] = pos_ctrl
|
||||
self.sim.forward()
|
||||
self.desired_pos = self.sim.data.get_site_xpos('wam/paddle_center').copy()
|
||||
|
||||
self.sim.data.ctrl[:] = pos_ctrl
|
||||
|
||||
def _is_success(self, achieved_goal, desired_goal):
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
render_mode = "human" # "human" or "partial" or "final"
|
||||
env = TableTennisEnv()
|
||||
env.reset()
|
||||
# env.render(mode=render_mode)
|
||||
|
||||
for i in range(500):
|
||||
# objective.load_result("/tmp/cma")
|
||||
# test with random actions
|
||||
ac = env.action_space.sample()
|
||||
# ac[0] += np.pi/2
|
||||
obs, rew, d, info = env.step(ac)
|
||||
env.render(mode=render_mode)
|
||||
|
||||
print(rew)
|
||||
|
||||
if d:
|
||||
break
|
||||
|
||||
env.close()
|
||||
Reference in New Issue
Block a user