Skip to content

Commit

Permalink
quasistatic iiwa grasping and pick-up demo (RobotLocomotion#8518)
Browse files Browse the repository at this point in the history 
* fixed debug mode assertion failures.

* removed manual label in test build rule

* iiwa pickup demo now works with new API.

* code style

* 1. checks final simulation state. 2. removed wrong comments from iiwa arm urdf.

* buildify build file

* passes Eigen::Quaternion to QuaternionToSpaceXYZ

* documented test criteria

* declare expected final position as const

* compares object final position and orientation to expected values.

* typo.

* final end effector pose becomes an input to MakePlan(.).

* replace deprecated rpy2quat

* changes iiwa urdf file name

* uses new RollPitchYaw class.

* minor changes

* replace tabs with spaces and add newline at EOF in .sdf files
  • Loading branch information
@pangtao22
pangtao22 authored Apr 27, 2018
1 parent b563541 commit 3edabff
Show file tree
Hide file tree
Showing 8 changed files with 1,130 additions and 2 deletions.
33 changes: 33 additions & 0 deletions manipulation/dev/BUILD.bazel
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,39 @@ drake_cc_binary(
],
)

drake_cc_binary(
name = "quasistatic_iiwa_pick_and_place_demo",
testonly = 1,
srcs = ["quasistatic_iiwa_pick_and_place_demo.cc"],
data = [
":double_dumbbell_for_pick_up.sdf",
":dumbbell_for_pick_up.sdf",
":models",
"//examples/kuka_iiwa_arm:models",
"//manipulation/models/iiwa_description:models",
],
deps = [
":quasistatic_system",
"//common:find_resource",
"//common/test_utilities:eigen_matrix_compare",
"//common/trajectories:piecewise_polynomial",
"//examples/kuka_iiwa_arm:iiwa_common",
"//lcm",
"//manipulation/util:sim_diagram_builder",
"//manipulation/util:world_sim_tree_builder",
"//math:geometric_transform",
"//systems/analysis",
"//systems/controllers:pid_controller",
"//systems/primitives:demultiplexer",
"//systems/primitives:matrix_gain",
"//systems/primitives:multiplexer",
"//systems/primitives:signal_logger",
"//systems/primitives:trajectory_source",
"@lcmtypes_bot2_core",
"@lcmtypes_robotlocomotion",
],
)

install_data()

add_lint_tests()
45 changes: 45 additions & 0 deletions manipulation/dev/box_as_table.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
<?xml version="1.0"?>
<sdf version='1.6'>
<model name='box_as_table'>

<link name="box">
<pose frame=''>0 0 0.4 0 -0 0</pose>
<visual>
<geometry>
<box>
<size>0.5 0.5 0.8</size>
</box>
</geometry>
<material name="green">
<ambient> 0.9 0.9 0.9 1</ambient>
<diffuse> 0.9 0.9 0.9 1</diffuse>
</material>
</visual>

<inertial>
<mass>0.988882</mass>
<inertia>
<ixx>0.162992</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.162992</iyy>
<iyz>0</iyz>
<izz>0.164814</izz>
</inertia>
</inertial>

<collision>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.5 0.5 0.8</size>
</box>
</geometry>
</collision>
</link>


</model>
</sdf>

128 changes: 128 additions & 0 deletions manipulation/dev/double_dumbbell_for_pick_up.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,128 @@
<?xml version="1.0"?>
<sdf version='1.6'>
<model name='dumbbell'>

<link name="dumbbell">
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.05</iyy>
<iyz>0</iyz>
<izz>0.05</izz>
</inertia>
</inertial>

<pose frame=''>0 0 0 0 -0 0</pose>
<!--
<visual>
<pose frame=''>0 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.015</radius>
<length>0.07</length>
</cylinder>
</geometry>
<material name="green">
<ambient> 0 1 0 1</ambient>
<diffuse> 0 1 0 1</diffuse>
</material>
</visual>
-->

<visual>
<pose frame=''>0.02 0.015 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.015</radius>
</sphere>
</geometry>
<material name="green">
<ambient> 0 1 0 1</ambient>
<diffuse> 0 1 0 1</diffuse>
</material>
</visual>

<visual>
<pose frame=''>0.02 -0.015 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.015</radius>
</sphere>
</geometry>
<material name="green">
<ambient> 0 1 0 1</ambient>
<diffuse> 0 1 0 1</diffuse>
</material>
</visual>

<visual>
<pose frame=''>-0.02 0.015 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.015</radius>
</sphere>
</geometry>
<material name="green">
<ambient> 0 1 0 1</ambient>
<diffuse> 0 1 0 1</diffuse>
</material>
</visual>

<visual>
<pose frame=''>-0.02 -0.015 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.015</radius>
</sphere>
</geometry>
<material name="green">
<ambient> 0 1 0 1</ambient>
<diffuse> 0 1 0 1</diffuse>
</material>
</visual>


<collision>
<pose frame=''>0.02 0.015 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.015</radius>
</sphere>
</geometry>
</collision>

<collision>
<pose frame=''>0.02 -0.015 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.015</radius>
</sphere>
</geometry>
</collision>

<collision>
<pose frame=''>-0.02 0.015 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.015</radius>
</sphere>
</geometry>
</collision>

<collision>
<pose frame=''>-0.02 -0.015 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.015</radius>
</sphere>
</geometry>
</collision>

</link>

</model>
</sdf>

79 changes: 79 additions & 0 deletions manipulation/dev/dumbbell_for_pick_up.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,79 @@
<?xml version="1.0"?>
<sdf version='1.6'>
<model name='dumbbell'>

<link name="dumbbell">
<inertial>
<mass>0.988882</mass>
<inertia>
<ixx>0.162992</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.162992</iyy>
<iyz>0</iyz>
<izz>0.164814</izz>
</inertia>
</inertial>

<pose frame=''>0 0 0 0 -0 0</pose>

<visual>
<pose frame=''>0 0 0 0 1.5708 0</pose>
<geometry>
<cylinder>
<radius>0.015</radius>
<length>0.07</length>
</cylinder>
</geometry>
<material name="green">
<color rgba="0 1 0 1" />
</material>
</visual>

<!--
<visual>
<pose frame=''>0.02 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.015</radius>
</sphere>
</geometry>
<material name="green">
<color rgba="0 1 0 1" />
</material>
</visual>
<visual>
<pose frame=''>-0.02 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.015</radius>
</sphere>
</geometry>
<material name="green">
<color rgba="0 1 0 1" />
</material>
</visual>
-->

<collision>
<pose frame=''>-0.02 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.015</radius>
</sphere>
</geometry>
</collision>

<collision>
<pose frame=''>0.02 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.015</radius>
</sphere>
</geometry>
</collision>

</link>

</model>
</sdf>
Loading

0 comments on commit 3edabff

Please sign in to comment.