diff --git a/models/box_30_40_40cm.sdf b/models/box_30_40_40cm.sdf
new file mode 100644
index 0000000..98e215f
--- /dev/null
+++ b/models/box_30_40_40cm.sdf
@@ -0,0 +1,115 @@
+
+
+
+
+
+
+ 5.0
+
+ 0.13333333333333336
+ 0
+ 0
+ 0.10416666666666666
+ 0
+ 0.10416666666666666
+
+
+
+
+
+
+ 0.30 0.40 0.40
+
+
+
+ 0.9 0.9 0.9 0.9
+
+
+
+
+
+
+ 0.30 0.40 0.40
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+
+ -0.15 -0.20 -0.20 0 0 0
+
+
+ 1e-7
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+
+ 0.15 -0.20 -0.20 0 0 0
+
+
+ 1e-7
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+
+ -0.15 0.20 -0.20 0 0 0
+
+
+ 1e-7
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+
+ 0.15 0.20 -0.20 0 0 0
+
+
+ 1e-7
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/models/dual_jaco_torso_sphere_collision_push.yml b/models/dual_jaco_torso_sphere_collision_push.yml
new file mode 100644
index 0000000..2973cf6
--- /dev/null
+++ b/models/dual_jaco_torso_sphere_collision_push.yml
@@ -0,0 +1,46 @@
+directives:
+- add_directives:
+ file: package://quasistatic_simulator/ground.yml
+
+- add_model:
+ name: jaco_left
+ file: package://quasistatic_simulator/j2s7s300_arm_sphere_link_collisions_v2.sdf
+
+- add_model:
+ name: jaco_right
+ file: package://quasistatic_simulator/j2s7s300_arm_sphere_link_collisions_v2.sdf
+
+- add_model:
+ name: torso
+ file: package://quasistatic_simulator/torso_0.4_0.1_0.5.sdf
+
+- add_frame:
+ name: world_left_arm_offset
+ X_PF:
+ base_frame: world
+ translation: [0, 0.28, 0]
+
+- add_frame:
+ name: world_right_arm_offset
+ X_PF:
+ base_frame: world
+ translation: [0, -0.28, 0]
+
+- add_frame:
+ name: world_torso
+ X_PF:
+ base_frame: world
+ translation: [-0.03, 0, 0.20]
+ rotation: !Rpy {deg: [0., 0.0,90.0]}
+
+- add_weld:
+ parent: world_left_arm_offset
+ child: jaco_left::base
+
+- add_weld:
+ parent: world_right_arm_offset
+ child: jaco_right::base
+
+- add_weld:
+ parent: world_torso
+ child: torso::base
\ No newline at end of file
diff --git a/models/dual_jaco_torso_sphere_collision_push_no_ground.yml b/models/dual_jaco_torso_sphere_collision_push_no_ground.yml
new file mode 100644
index 0000000..c2c5f89
--- /dev/null
+++ b/models/dual_jaco_torso_sphere_collision_push_no_ground.yml
@@ -0,0 +1,43 @@
+directives:
+- add_model:
+ name: jaco_left
+ file: package://quasistatic_simulator/j2s7s300_arm_sphere_link_collisions_v2.sdf
+
+- add_model:
+ name: jaco_right
+ file: package://quasistatic_simulator/j2s7s300_arm_sphere_link_collisions_v2.sdf
+
+- add_model:
+ name: torso
+ file: package://quasistatic_simulator/torso_0.4_0.1_0.5.sdf
+
+- add_frame:
+ name: world_left_arm_offset
+ X_PF:
+ base_frame: world
+ translation: [0, 0.28, 0]
+
+- add_frame:
+ name: world_right_arm_offset
+ X_PF:
+ base_frame: world
+ translation: [0, -0.28, 0]
+
+- add_frame:
+ name: world_torso
+ X_PF:
+ base_frame: world
+ translation: [-0.03, 0, 0.20]
+ rotation: !Rpy {deg: [0., 0.0, 90.0]}
+
+- add_weld:
+ parent: world_left_arm_offset
+ child: jaco_left::base
+
+- add_weld:
+ parent: world_right_arm_offset
+ child: jaco_right::base
+
+- add_weld:
+ parent: world_torso
+ child: torso::base
\ No newline at end of file
diff --git a/models/j2s7s300_arm_sphere_collision_v2.sdf b/models/j2s7s300_arm_sphere_collision_v2.sdf
index af578ef..92d6e89 100644
--- a/models/j2s7s300_arm_sphere_collision_v2.sdf
+++ b/models/j2s7s300_arm_sphere_collision_v2.sdf
@@ -4,7 +4,7 @@
SDF automatically generated with: gz sdf -p /my_urdf.urdf > /my_sdf.sdf
Later edited by hand to:
-* add collision geometries
+* add collision geometries to end effector link.
* add velocity and effort limits
-->
diff --git a/models/j2s7s300_arm_sphere_link_collisions_v2.sdf b/models/j2s7s300_arm_sphere_link_collisions_v2.sdf
new file mode 100644
index 0000000..5ea7c77
--- /dev/null
+++ b/models/j2s7s300_arm_sphere_link_collisions_v2.sdf
@@ -0,0 +1,692 @@
+
+
+
+
+
+ 0 0 0 0 -0 0
+
+ 0 0 0.1255 0 -0 0
+ 0.46784
+
+ 0.000951271
+ 0
+ 0
+ 0.000951271
+ 0
+ 0.000374272
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1 1 1
+ package://drake/manipulation/models/jaco_description/meshes/base.obj
+
+
+
+ 0.1788232 0.1788232 0.1788232 0.8
+
+
+
+
+ 0 0 0.15675 3.14159 -0 3.14159
+
+ 0 -0.002 -0.0605 0 -0 0
+ 0.7477
+
+ 0.00152032
+ 0
+ 0
+ 0.00152032
+ 0
+ 0.00059816
+
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1 1 1
+ package://drake/manipulation/models/jaco_description/meshes/shoulder.obj
+
+
+
+ 0.1788232 0.1788232 0.1788232 0.8
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1 1 1
+ package://drake/manipulation/models/jaco_description/meshes/ring_big.obj
+
+
+
+ 0.627451 0.627451 0.627451 0.8
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 0.045
+
+
+
+ 0.8 0.8 0.8 1.0
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ 0.045
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+
+
+ j2s7s300_link_1
+ base
+
+ -0 0 -1
+
+ -6.28
+ 6.28
+ 40
+ 0.63
+
+
+ 0
+ 0
+ 0
+ 0
+
+ 1
+
+
+
+ 0 0.0016 0.2755 1.5708 0 -0
+
+ 0 -0.103563 0 0 -0 0
+ 0.8447
+
+ 0.00247074
+ 0
+ 0
+ 0.000380115
+ 0
+ 0.00247074
+
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1 1 1
+ package://drake/manipulation/models/jaco_description/meshes/arm_half_1.obj
+
+
+
+ 0.1788232 0.1788232 0.1788232 0.8
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1 1 1
+ package://drake/manipulation/models/jaco_description/meshes/ring_big.obj
+
+
+
+ 0.627451 0.627451 0.627451 0.8
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 0.045
+
+
+
+ 0.8 0.8 0.8 1.0
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ 0.045
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+
+
+ j2s7s300_link_2
+ j2s7s300_link_1
+
+ -0 -1 -0
+
+ 0.820305
+ 5.46288
+ 80
+ 0.63
+
+
+ 0
+ 0
+ 0
+ 0
+
+ 1
+
+
+
+ -0 0.0016 0.0705 0 0 -0
+
+ 0 0 -0.102245 0 -0 0
+ 0.8447
+
+ 0.00247074
+ 0
+ 0
+ 0.00247074
+ 0
+ 0.000380115
+
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1 1 1
+ package://drake/manipulation/models/jaco_description/meshes/arm_half_2.obj
+
+
+
+ 0.1788232 0.1788232 0.1788232 0.8
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1 1 1
+ package://drake/manipulation/models/jaco_description/meshes/ring_big.obj
+
+
+
+ 0.627451 0.627451 0.627451 0.8
+
+
+
+ 0 0.015 -0.09 0 -0 0
+
+
+ 0.025
+
+
+
+ 0.8 0.8 0.8 1.0
+
+
+
+ 0 0 -0.012 0 -0 0
+
+
+ 0.045
+
+
+
+ 0.8 0.8 0.8 1.0
+
+
+
+
+ 0 0.015 -0.09 0 0 0
+
+
+ 0.025
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+
+ 0 0 -0.012 0 0 0
+
+
+ 0.045
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+
+ j2s7s300_link_3
+ j2s7s300_link_2
+
+ 0 -0 1
+
+ -6.28
+ 6.28
+ 40
+ 0.63
+
+
+ 0
+ 0
+ 0
+ 0
+
+ 1
+
+
+
+ -0 0.0016 -0.1345 1.5708 -0 -3.14159
+
+ 0 0.081 -0.0086 0 -0 0
+ 0.6763
+
+ 1
+ 0
+ 0
+ 1
+ 0
+ 1
+
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1 1 1
+ package://drake/manipulation/models/jaco_description/meshes/forearm.obj
+
+
+
+ 0.1788232 0.1788232 0.1788232 0.8
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1 1 1
+ package://drake/manipulation/models/jaco_description/meshes/ring_small.obj
+
+
+
+ 0.627451 0.627451 0.627451 0.8
+
+
+
+ 0 0.15 -0.015 0 -0 0
+
+
+ 0.025
+
+
+
+ 0.8 0.8 0.8 1.0
+
+
+
+ 0 0 0.0 0 -0 0
+
+
+ 0.045
+
+
+
+ 0.8 0.8 0.8 1.0
+
+
+
+
+ 0 0.15 -0.015 0 0 0
+
+
+ 0.025
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ 0.045
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+
+
+ j2s7s300_link_4
+ j2s7s300_link_3
+
+ 0 1 0
+
+ 0.523599
+ 5.75959
+ 40
+ 0.63
+
+
+ 0
+ 0
+ 0
+ 0
+
+ 1
+
+
+
+ -0 -0.0098 0.0728 -3.14159 0 0
+
+ 0 0.002885 -0.054193 0 -0 0
+ 0.463
+
+ 0.000432132
+ 0
+ 0
+ 0.000432132
+ 0
+ 9.26e-05
+
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1 1 1
+ package://drake/manipulation/models/jaco_description/meshes/wrist_spherical_1.obj
+
+
+
+ 0.1788232 0.1788232 0.1788232 0.8
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1 1 1
+ package://drake/manipulation/models/jaco_description/meshes/ring_small.obj
+
+
+
+ 0.627451 0.627451 0.627451 0.8
+
+
+
+ 0 0 -0.012 0 -0 0
+
+
+ 0.035
+
+
+
+ 0.8 0.8 0.8 1.0
+
+
+
+
+ 0 0 -0.012 0 0 0
+
+
+ 0.035
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+
+
+ j2s7s300_link_5
+ j2s7s300_link_4
+
+ -0 0 -1
+
+ -6.28
+ 6.28
+ 20
+ 0.85
+
+
+ 0
+ 0
+ 0
+ 0
+
+ 1
+
+
+
+ 0 -0.0098 0.17655 -1.5708 -0 3.14159
+
+ 0 0.049721 -0.002856 0 -0 0
+ 0.463
+
+ 0.000432132
+ 0
+ 0
+ 9.26e-05
+ 0
+ 0.000432132
+
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1 1 1
+ package://drake/manipulation/models/jaco_description/meshes/wrist_spherical_2.obj
+
+
+
+ 0.1788232 0.1788232 0.1788232 0.8
+
+
+
+ 0 0 0 0 -0 0
+
+
+ 1 1 1
+ package://drake/manipulation/models/jaco_description/meshes/ring_small.obj
+
+
+
+ 0.627451 0.627451 0.627451 0.8
+
+
+
+ 0 0 -0.015 0 -0 0
+
+
+ 0.035
+
+
+
+ 0.8 0.8 0.8 1.0
+
+
+
+
+ 0 0 -0.015 0 0 0
+
+
+ 0.035
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+
+
+ j2s7s300_link_6
+ j2s7s300_link_5
+
+ -0 -1 -0
+
+ 1.13446
+ 5.14872
+ 20
+ 0.85
+
+
+ 0
+ 0
+ 0
+ 0
+
+ 1
+
+
+
+ -0 -0.0098 0.0728 0 0 -0
+
+ 0 0 -0.06 0 -0 0
+ 0.001
+
+ 1
+ 0
+ 0
+ 1
+ 0
+ 1
+
+
+
+
+ 0 0 0.03 0 -0 0
+
+
+ 0.031
+
+
+
+ 0.8 0.8 0.8 1.0
+
+
+
+
+ 0 0 0.03 0 0 0
+
+
+ 0.031
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+
+
+ j2s7s300_link_7
+ j2s7s300_link_6
+
+ 0 -0 1
+
+ -6.28
+ 6.28
+ 20
+ 0.85
+
+
+ 0
+ 0
+ 0
+ 0
+
+ 1
+
+
+
+
diff --git a/models/q_sys/dual_jaco_torso_box.yml b/models/q_sys/dual_jaco_torso_box.yml
new file mode 100644
index 0000000..ca070e8
--- /dev/null
+++ b/models/q_sys/dual_jaco_torso_box.yml
@@ -0,0 +1,20 @@
+model_directive: package://quasistatic_simulator/dual_jaco_torso_sphere_collision_push.yml
+robots:
+ -
+ name: jaco_left
+ Kp: [800, 600, 600, 600, 400, 200, 200]
+ -
+ name: jaco_right
+ Kp: [800, 600, 600, 600, 400, 200, 200]
+
+objects:
+ -
+ name: box
+ file: package://quasistatic_simulator/box_30_40_40cm.sdf
+
+quasistatic_sim_params:
+ gravity: [0, 0, -9.8]
+ nd_per_contact: 4
+ contact_detection_tolerance: 0.2
+ is_quasi_dynamic: True
+ unactuated_mass_scale: 5
diff --git a/models/q_sys/dual_jaco_torso_box_no_ground.yml b/models/q_sys/dual_jaco_torso_box_no_ground.yml
new file mode 100644
index 0000000..934f6ed
--- /dev/null
+++ b/models/q_sys/dual_jaco_torso_box_no_ground.yml
@@ -0,0 +1,20 @@
+model_directive: package://quasistatic_simulator/dual_jaco_torso_sphere_collision_push_no_ground.yml
+robots:
+ -
+ name: jaco_left
+ Kp: [800, 600, 600, 600, 400, 200, 200]
+ -
+ name: jaco_right
+ Kp: [800, 600, 600, 600, 400, 200, 200]
+
+objects:
+ -
+ name: box
+ file: package://quasistatic_simulator/box_30_40_40cm.sdf
+
+quasistatic_sim_params:
+ gravity: [0, 0, 0]
+ nd_per_contact: 4
+ contact_detection_tolerance: 0.2
+ is_quasi_dynamic: True
+ unactuated_mass_scale: 5
diff --git a/models/torso_0.4_0.1_0.5.sdf b/models/torso_0.4_0.1_0.5.sdf
new file mode 100644
index 0000000..8dd277d
--- /dev/null
+++ b/models/torso_0.4_0.1_0.5.sdf
@@ -0,0 +1,39 @@
+
+
+
+
+ 0 0 0 0 0 0
+
+
+ 0 0 0.05 0 0 0
+
+
+ 0.4 0.1 0.5
+
+
+
+ 0.1788232 0.1788232 0.1788232 0.8
+
+
+
+
+ 0 0 0.05 0 0 0
+
+
+ 0.4 0.1 0.5
+
+
+
+
+
+
+ 0.5
+ 0.5
+
+
+
+
+
+
+
+
\ No newline at end of file