def test_id_table(self):
robot = RigidBodyTree()
id_table = AddModelInstancesFromSdfFile(
FindResourceOrThrow("drake/examples/acrobot/Acrobot.sdf"),
FloatingBaseType.kRollPitchYaw, None, robot)
# Check IDs.
(name, id), = id_table.items()
self.assertEqual(name, "Acrobot")
self.assertEqual(id, 0)
# Ensure that we have our desired base body.
base_body_id, = robot.FindBaseBodies(id)
expected_body_id = robot.FindBody("base_link").get_body_index()
self.assertEqual(base_body_id, expected_body_id)
def test_id_table(self):
robot = RigidBodyTree()
id_table = AddModelInstancesFromSdfFile(
FindResourceOrThrow("drake/examples/acrobot/Acrobot.sdf"),
FloatingBaseType.kRollPitchYaw, None, robot)
# Check IDs.
(name, id), = id_table.items()
self.assertEqual(name, "Acrobot")
self.assertEqual(id, 0)
# Ensure that we have our desired base body.
base_body_id, = robot.FindBaseBodies(id)
expected_body_id = robot.FindBody("base_link").get_body_index()
self.assertEqual(base_body_id, expected_body_id)
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("filename")
args = parser.parse_args()
# Load the model file.
tree = RigidBodyTree()
ext = os.path.splitext(args.filename)[1]
if ext == ".sdf":
AddModelInstancesFromSdfFile(args.filename, FloatingBaseType.kFixed,
None, tree)
elif ext == ".urdf":
AddModelInstanceFromUrdfFile(args.filename, FloatingBaseType.kFixed,
None, tree)
else:
parser.error("Unknown extension " + ext)
# Send drake-visualizer messages to load the model and then position it in
# its default configuration.
q = tree.getZeroConfiguration()
v = np.zeros(tree.get_num_velocities())
lcm = DrakeLcm()
visualizer = DrakeVisualizer(tree=tree, lcm=lcm)
visualizer.PublishLoadRobot()
context = visualizer.CreateDefaultContext()
context.FixInputPort(0, BasicVector(np.concatenate([q, v])))
visualizer.Publish(context)
def load_robot(filename, **kwargs):
robot = RigidBodyTree()
if filename.endswith(".sdf"):
AddModelInstancesFromSdfFile(FindResourceOrThrow(filename),
FloatingBaseType.kRollPitchYaw,
None, robot, **kwargs)
else:
assert filename.endswith(".urdf")
AddModelInstanceFromUrdfFile(FindResourceOrThrow(filename),
FloatingBaseType.kRollPitchYaw,
None, robot, **kwargs)
return robot
def test_sdf(self):
sdf_file = os.path.join(getDrakePath(), "examples/acrobot/Acrobot.sdf")
with open(sdf_file) as f:
sdf_string = f.read()
package_map = PackageMap()
weld_frame = None
floating_base_type = FloatingBaseType.kRollPitchYaw
robot_1 = RigidBodyTree()
AddModelInstancesFromSdfStringSearchingInRosPackages(
sdf_string, package_map, floating_base_type, weld_frame, robot_1)
robot_2 = RigidBodyTree()
AddModelInstancesFromSdfString(sdf_string, floating_base_type,
weld_frame, robot_2)
robot_3 = RigidBodyTree()
AddModelInstancesFromSdfFile(sdf_file, floating_base_type, weld_frame,
robot_3)
for robot in robot_1, robot_2, robot_3:
expected_num_bodies = 4
self.assertEqual(robot.get_num_bodies(), expected_num_bodies)
def MakeControlledKukaPlan():
kSdfPath = "drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf"
num_q = 7
rigid_body_tree = RigidBodyTree()
AddModelInstancesFromSdfFile(FindResourceOrThrow(kSdfPath),
FloatingBaseType.kFixed, None,
rigid_body_tree)
zero_conf = rigid_body_tree.getZeroConfiguration()
joint_lb = zero_conf - np.ones(num_q) * 0.01
joint_ub = zero_conf + np.ones(num_q) * 0.01
pc1 = PostureConstraint(rigid_body_tree, np.array([0, 0.5]))
joint_idx = np.arange(num_q)
pc1.setJointLimits(joint_idx, joint_lb, joint_ub)
pos_end = np.array([0.6, 0, 0.325])
pos_lb = pos_end - np.ones(3) * 0.005
pos_ub = pos_end + np.ones(3) * 0.005
wpc1 = WorldPositionConstraint(
rigid_body_tree, rigid_body_tree.FindBodyIndex("iiwa_link_7"),
np.array([0, 0, 0]), pos_lb, pos_ub, np.array([1, 3]))
pc2 = PostureConstraint(rigid_body_tree, np.array([4, 5.9]))
pc2.setJointLimits(joint_idx, joint_lb, joint_ub)
wpc2 = WorldPositionConstraint(
rigid_body_tree, rigid_body_tree.FindBodyIndex("iiwa_link_7"),
np.array([0, 0, 0]), pos_lb, pos_ub, np.array([6, 9]))
joint_position_start_idx = rigid_body_tree.FindChildBodyOfJoint(
"iiwa_joint_2").get_position_start_index()
pc3 = PostureConstraint(rigid_body_tree, np.array([6, 8]))
pc3.setJointLimits(np.array([joint_position_start_idx]),
np.ones(1) * 0.7,
np.ones(1) * 0.8)
kTimes = np.array([0.0, 2.0, 5.0, 7.0, 9.0])
q_seed = np.zeros((rigid_body_tree.get_num_positions(), kTimes.size))
q_norm = np.zeros((rigid_body_tree.get_num_positions(), kTimes.size))
for i in range(kTimes.size):
q_seed[:, i] = zero_conf + 0.1 * np.ones(
rigid_body_tree.get_num_positions())
q_norm[:, i] = zero_conf
ikoptions = IKoptions(rigid_body_tree)
constraint_array = [pc1, wpc1, pc2, pc3, wpc2]
ik_results = InverseKinPointwise(rigid_body_tree, kTimes, q_seed, q_norm,
constraint_array, ikoptions)
q_sol = ik_results.q_sol
ik_info = ik_results.info
infeasible_constraint = ik_results.infeasible_constraints
info_good = True
for i in range(kTimes.size):
print('IK ik_info[{}] = {}'.format(i, ik_info[i]))
if ik_info[i] != 1:
info_good = False
if not info_good:
raise Exception(
"inverseKinPointwise failed to compute a valid solution.")
knots = np.array(q_sol).transpose()
traj_pp = PiecewisePolynomial.FirstOrderHold(kTimes, knots)
return traj_pp
print(context.get_time())
output.SetFromVector(ret)
poseOutput = builder.AddSystem(PoseOutput())
# 3.Signal logger
logger_pose = builder.AddSystem(SignalLogger(3))
builder.Connect(poseOutput.get_output_port(0), logger_pose.get_input_port(0))
kSdfPath = "drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf"
rigid_body_tree = RigidBodyTree()
AddModelInstancesFromSdfFile(FindResourceOrThrow(kSdfPath),
FloatingBaseType.kFixed,
None,
rigid_body_tree
)
def kuka_PID_params():
num_joint_kuka = 7
kp = np.array([100, 100, 100, 100, 100, 100, 100])
ki = np.zeros(num_joint_kuka)
kd = 2 * np.sqrt(kp)
assert kp.shape[0] == num_joint_kuka
return kp, ki, kd
iiwa_kp,iiwa_ki,iiwa_kd = kuka_PID_params()
