def test_free_joint_indices():
free_joint_indices = [1, 2, 4, 5]
with px.Client(mode=p.DIRECT):
robot = px.Robot("kuka_iiwa/model.urdf")
robot.free_joint_indices = free_joint_indices
states = robot.get_states()
assert robot.num_dofs == len(free_joint_indices)
assert states.joint_position.shape == (robot.num_dofs,)
assert states.joint_velocity.shape == (robot.num_dofs,)
assert states.joint_reaction_forces.shape == (robot.num_dofs, 6)
assert states.applied_joint_motor_torque.shape == (robot.num_dofs,)
S = robot.state_space
assert S.joint_position.shape == states.joint_position.shape
assert S.joint_velocity.shape == states.joint_velocity.shape
assert S.joint_reaction_forces.shape == states.joint_reaction_forces.shape
assert (
S.applied_joint_motor_torque.shape
== states.applied_joint_motor_torque.shape
)
A = robot.action_space
assert A.joint_position.shape == (robot.num_dofs,)
def test_pybullet_body():
with px.Client(mode=p.DIRECT):
init_position = (0, 0, 1)
init_orientation = (0, 0, 0, 1)
robot = px.Robot("kuka_iiwa/model.urdf", init_position,
init_orientation)
print(robot.free_joint_indices)
def main():
px.init()
robot = px.Robot("kuka_iiwa/model.urdf")
# Run the simulation in another thread
t = px.utils.SimulationThread(1.0)
t.start()
# ControlPanel also takes pybulletX.Body
panel = px.gui.RobotControlPanel(robot)
panel.start()
input("Press any key to exit ...")
def main():
px.init()
robot = px.Robot("kuka_iiwa/model.urdf", use_fixed_base=True)
robot.torque_control = True
while True:
time.sleep(0.01)
error = desired_joint_positions - robot.get_states().joint_position
actions = robot.action_space.new()
actions.joint_torque = error * P_GAIN
robot.set_actions(actions)
p.stepSimulation()
def test_robot_state_space_configuration(joint_position, joint_velocity,
joint_reaction_forces,
applied_joint_motor_torque):
with px.Client(mode=p.DIRECT):
robot = px.Robot("kuka_iiwa/model.urdf")
robot.configure_state_space(
joint_position,
joint_velocity,
joint_reaction_forces,
applied_joint_motor_torque,
)
for S in [robot.state_space, robot.get_states()]:
assert joint_position == hasattr(S, "joint_position")
assert joint_velocity == hasattr(S, "joint_velocity")
assert joint_reaction_forces == hasattr(S, "joint_reaction_forces")
assert applied_joint_motor_torque == hasattr(
S, "applied_joint_motor_torque")