diff --git a/docs/sphinx/getting_started/manipulation.rst b/docs/sphinx/getting_started/manipulation.rst index 11eafd3e5..d4c0623a7 100644 --- a/docs/sphinx/getting_started/manipulation.rst +++ b/docs/sphinx/getting_started/manipulation.rst @@ -33,19 +33,21 @@ It shows the following functionalities: .. code-block:: python - import time import enum - import numpy as np - import gym_ignition - from typing import List + import time from functools import partial + from typing import List + + import gym_ignition import gym_ignition_environments + import numpy as np + from gym_ignition.context.gazebo import controllers from gym_ignition.rbd import conversions + from gym_ignition.rbd.idyntree import inverse_kinematics_nlp + from scipy.spatial.transform import Rotation as R + from scenario import core as scenario_core from scenario import gazebo as scenario_gazebo - from scipy.spatial.transform import Rotation as R - from gym_ignition.context.gazebo import controllers - from gym_ignition.rbd.idyntree import inverse_kinematics_nlp # Configure verbosity scenario_gazebo.set_verbosity(scenario_gazebo.Verbosity_error) @@ -54,26 +56,31 @@ It shows the following functionalities: np.set_printoptions(precision=4, suppress=True) - def add_panda_controller(panda: gym_ignition_environments.models.panda.Panda, - controller_period: float) -> None: + def add_panda_controller( + panda: gym_ignition_environments.models.panda.Panda, controller_period: float + ) -> None: # Set the controller period assert panda.set_controller_period(period=controller_period) # Increase the max effort of the fingers - panda.get_joint(joint_name="panda_finger_joint1").to_gazebo(). \ - set_max_generalized_force(max_force=500.0) - panda.get_joint(joint_name="panda_finger_joint2").to_gazebo(). \ - set_max_generalized_force(max_force=500.0) + panda.get_joint( + joint_name="panda_finger_joint1" + ).to_gazebo().set_max_generalized_force(max_force=500.0) + panda.get_joint( + joint_name="panda_finger_joint2" + ).to_gazebo().set_max_generalized_force(max_force=500.0) # Insert the ComputedTorqueFixedBase controller - assert panda.to_gazebo().insert_model_plugin(*controllers.ComputedTorqueFixedBase( - kp=[100.0] * (panda.dofs() - 2) + [10000.0] * 2, - ki=[0.0] * panda.dofs(), - kd=[17.5] * (panda.dofs() - 2) + [100.0] * 2, - urdf=panda.get_model_file(), - joints=panda.joint_names(), - ).args()) + assert panda.to_gazebo().insert_model_plugin( + *controllers.ComputedTorqueFixedBase( + kp=[100.0] * (panda.dofs() - 2) + [10000.0] * 2, + ki=[0.0] * panda.dofs(), + kd=[17.5] * (panda.dofs() - 2) + [100.0] * 2, + urdf=panda.get_model_file(), + joints=panda.joint_names(), + ).args() + ) # Initialize the controller to the current state assert panda.set_joint_position_targets(panda.joint_positions()) @@ -81,34 +88,40 @@ It shows the following functionalities: assert panda.set_joint_acceleration_targets(panda.joint_accelerations()) - def get_panda_ik(panda: gym_ignition_environments.models.panda.Panda, - optimized_joints: List[str]) -> \ - inverse_kinematics_nlp.InverseKinematicsNLP: + def get_panda_ik( + panda: gym_ignition_environments.models.panda.Panda, optimized_joints: List[str] + ) -> inverse_kinematics_nlp.InverseKinematicsNLP: # Create IK ik = inverse_kinematics_nlp.InverseKinematicsNLP( urdf_filename=panda.get_model_file(), considered_joints=optimized_joints, - joint_serialization=panda.joint_names()) + joint_serialization=panda.joint_names(), + ) # Initialize IK - ik.initialize(verbosity=1, - floating_base=False, - cost_tolerance=1E-8, - constraints_tolerance=1E-8, - base_frame=panda.base_frame()) + ik.initialize( + verbosity=1, + floating_base=False, + cost_tolerance=1e-8, + constraints_tolerance=1e-8, + base_frame=panda.base_frame(), + ) # Set the current configuration ik.set_current_robot_configuration( base_position=np.array(panda.base_position()), base_quaternion=np.array(panda.base_orientation()), - joint_configuration=np.array(panda.joint_positions())) + joint_configuration=np.array(panda.joint_positions()), + ) # Add the cartesian target of the end effector end_effector = "end_effector_frame" - ik.add_target(frame_name=end_effector, - target_type=inverse_kinematics_nlp.TargetType.POSE, - as_constraint=False) + ik.add_target( + frame_name=end_effector, + target_type=inverse_kinematics_nlp.TargetType.POSE, + as_constraint=False, + ) return ik @@ -120,17 +133,20 @@ It shows the following functionalities: # Download the cube SDF file bucket_sdf = scenario_gazebo.get_model_file_from_fuel( - uri=uri(org="GoogleResearch", - name="Threshold_Basket_Natural_Finish_Fabric_Liner_Small"), - use_cache=False) + uri=uri( + org="GoogleResearch", + name="Threshold_Basket_Natural_Finish_Fabric_Liner_Small", + ), + use_cache=False, + ) # Assign a custom name to the model model_name = "bucket" # Insert the model - assert world.insert_model(bucket_sdf, - scenario_core.Pose([0.68, 0, 1.02], [1., 0, 0, 1]), - model_name) + assert world.insert_model( + bucket_sdf, scenario_core.Pose([0.68, 0, 1.02], [1.0, 0, 0, 1]), model_name + ) # Return the model return world.get_model(model_name=model_name) @@ -143,56 +159,61 @@ It shows the following functionalities: # Download the cube SDF file bucket_sdf = scenario_gazebo.get_model_file_from_fuel( - uri=uri(org="OpenRobotics", - name="Table"), - use_cache=False) + uri=uri(org="OpenRobotics", name="Table"), use_cache=False + ) # Assign a custom name to the model model_name = "table" # Insert the model - assert world.insert_model(bucket_sdf, - scenario_core.Pose_identity(), - model_name) + assert world.insert_model(bucket_sdf, scenario_core.Pose_identity(), model_name) # Return the model return world.get_model(model_name=model_name) - def insert_cube_in_operating_area(world: scenario_gazebo.World) -> scenario_gazebo.Model: + def insert_cube_in_operating_area( + world: scenario_gazebo.World, + ) -> scenario_gazebo.Model: # Insert objects from Fuel uri = lambda org, name: f"https://fuel.ignitionrobotics.org/{org}/models/{name}" # Download the cube SDF file cube_sdf = scenario_gazebo.get_model_file_from_fuel( - uri=uri(org="openrobotics", name="wood cube 5cm"), use_cache=False) + uri=uri(org="openrobotics", name="wood cube 5cm"), use_cache=False + ) # Sample a random position random_position = np.random.uniform(low=[0.2, -0.3, 1.01], high=[0.4, 0.3, 1.01]) # Get a unique name model_name = gym_ignition.utils.scenario.get_unique_model_name( - world=world, model_name="cube") + world=world, model_name="cube" + ) # Insert the model assert world.insert_model( - cube_sdf, scenario_core.Pose(random_position, [1., 0, 0, 0]), model_name) + cube_sdf, scenario_core.Pose(random_position, [1.0, 0, 0, 0]), model_name + ) # Return the model return world.get_model(model_name=model_name) - def solve_ik(target_position: np.ndarray, - target_orientation: np.ndarray, - ik: inverse_kinematics_nlp.InverseKinematicsNLP) -> np.ndarray: + def solve_ik( + target_position: np.ndarray, + target_orientation: np.ndarray, + ik: inverse_kinematics_nlp.InverseKinematicsNLP, + ) -> np.ndarray: quat_xyzw = R.from_euler(seq="y", angles=90, degrees=True).as_quat() ik.update_transform_target( target_name=ik.get_active_target_names()[0], position=target_position, - quaternion=conversions.Quaternion.to_wxyz(xyzw=quat_xyzw)) + quaternion=conversions.Quaternion.to_wxyz(xyzw=quat_xyzw), + ) # Run the IK ik.solve() @@ -200,17 +221,21 @@ It shows the following functionalities: return ik.get_reduced_solution().joint_configuration - def end_effector_reached(position: np.array, - end_effector_link: scenario_core.Link, - max_error_pos: float = 0.01, - max_error_vel: float = 0.5, - mask: np.ndarray = np.array([1., 1., 1.])) -> bool: + def end_effector_reached( + position: np.array, + end_effector_link: scenario_core.Link, + max_error_pos: float = 0.01, + max_error_vel: float = 0.5, + mask: np.ndarray = np.array([1.0, 1.0, 1.0]), + ) -> bool: masked_target = mask * position masked_current = mask * np.array(end_effector_link.position()) - return np.linalg.norm(masked_current - masked_target) < max_error_pos and \ - np.linalg.norm(end_effector_link.world_linear_velocity()) < max_error_vel + return ( + np.linalg.norm(masked_current - masked_target) < max_error_pos + and np.linalg.norm(end_effector_link.world_linear_velocity()) < max_error_vel + ) def get_unload_position(bucket: scenario_core.Model) -> np.ndarray: @@ -224,8 +249,9 @@ It shows the following functionalities: CLOSE = enum.auto() - def move_fingers(panda: gym_ignition_environments.models.panda.Panda, - action: FingersAction) -> None: + def move_fingers( + panda: gym_ignition_environments.models.panda.Panda, action: FingersAction + ) -> None: # Get the joints of the fingers finger1 = panda.get_joint(joint_name="panda_finger_joint1") @@ -246,7 +272,8 @@ It shows the following functionalities: # Get the simulator and the world gazebo, world = gym_ignition.utils.scenario.init_gazebo_sim( - step_size=0.001, real_time_factor=2.0, steps_per_run=1) + step_size=0.001, real_time_factor=2.0, steps_per_run=1 + ) # Open the GUI gazebo.gui() @@ -255,7 +282,11 @@ It shows the following functionalities: # Insert the Panda manipulator panda = gym_ignition_environments.models.panda.Panda( - world=world, position=[-0.1, 0, 1.0]) + world=world, position=[-0.1, 0, 1.0] + ) + + # Disable joint velocity limits + _ = [j.to_gazebo().set_velocity_limit(1_000) for j in panda.joints()] # Enable contacts only for the finger links panda.get_link("panda_leftfinger").to_gazebo().enable_contact_detection(True) @@ -277,7 +308,9 @@ It shows the following functionalities: gazebo.run(paused=True) # Create and configure IK for the panda - ik_joints = [j.name() for j in panda.joints() if j.type is not scenario_core.JointType_fixed ] + ik_joints = [ + j.name() for j in panda.joints() if j.type is not scenario_core.JointType_fixed + ] ik = get_panda_ik(panda=panda, optimized_joints=ik_joints) # Get some manipulator links @@ -304,7 +337,8 @@ It shows the following functionalities: over_joint_configuration = solve_ik( target_position=position_over_cube, target_orientation=np.array(cube.base_orientation()), - ik=ik) + ik=ik, + ) # Set the joint references assert panda.set_joint_position_targets(over_joint_configuration, ik_joints) @@ -313,10 +347,12 @@ It shows the following functionalities: panda.open_fingers() # Run the simulation until the EE reached the desired position - while not end_effector_reached(position=position_over_cube, - end_effector_link=end_effector_frame, - max_error_pos=0.05, - max_error_vel=0.5): + while not end_effector_reached( + position=position_over_cube, + end_effector_link=end_effector_frame, + max_error_pos=0.05, + max_error_vel=0.5, + ): gazebo.run() # Wait a bit more @@ -332,7 +368,8 @@ It shows the following functionalities: over_joint_configuration = solve_ik( target_position=np.array(cube.base_position()) + np.array([0, 0, 0.04]), target_orientation=np.array(cube.base_orientation()), - ik=ik) + ik=ik, + ) # Set the joint references assert panda.set_joint_position_targets(over_joint_configuration, ik_joints) @@ -340,8 +377,9 @@ It shows the following functionalities: # Run the simulation until the EE reached the desired position while not end_effector_reached( - position=np.array(cube.base_position()) + np.array([0, 0, 0.04]), - end_effector_link=end_effector_frame): + position=np.array(cube.base_position()) + np.array([0, 0, 0.04]), + end_effector_link=end_effector_frame, + ): gazebo.run() @@ -358,8 +396,10 @@ It shows the following functionalities: panda.close_fingers() # Detect a graps reading the contact wrenches of the finger links - while not (np.linalg.norm(finger_left.contact_wrench()) >= 50.0 and - np.linalg.norm(finger_right.contact_wrench()) >= 50.0): + while not ( + np.linalg.norm(finger_left.contact_wrench()) >= 50.0 + and np.linalg.norm(finger_right.contact_wrench()) >= 50.0 + ): gazebo.run() # ============= @@ -375,16 +415,19 @@ It shows the following functionalities: over_joint_configuration = solve_ik( target_position=position_over_cube, target_orientation=np.array(cube.base_orientation()), - ik=ik) + ik=ik, + ) # Set the joint references assert panda.set_joint_position_targets(over_joint_configuration, ik_joints) # Run the simulation until the EE reached the desired position - while not end_effector_reached(position=position_over_cube, - end_effector_link=end_effector_frame, - max_error_pos=0.1, - max_error_vel=0.5): + while not end_effector_reached( + position=position_over_cube, + end_effector_link=end_effector_frame, + max_error_pos=0.1, + max_error_vel=0.5, + ): gazebo.run() # Wait a bit more @@ -400,20 +443,21 @@ It shows the following functionalities: unload_joint_configuration = solve_ik( target_position=get_unload_position(bucket=bucket), target_orientation=np.array([0, 1.0, 0, 0]), - ik=ik) + ik=ik, + ) # Set the joint references - assert panda.set_joint_position_targets(unload_joint_configuration, - ik_joints) + assert panda.set_joint_position_targets(unload_joint_configuration, ik_joints) # Run the simulation until the EE reached the desired position while not end_effector_reached( - position=get_unload_position(bucket=bucket) + - np.random.uniform(low=-0.05, high=0.05, size=3), - end_effector_link=end_effector_frame, - max_error_pos=0.01, - max_error_vel=0.1, - mask=np.array([1, 1, 0])): + position=get_unload_position(bucket=bucket) + + np.random.uniform(low=-0.05, high=0.05, size=3), + end_effector_link=end_effector_frame, + max_error_pos=0.01, + max_error_vel=0.1, + mask=np.array([1, 1, 0]), + ): gazebo.run() diff --git a/examples/panda_pick_and_place.py b/examples/panda_pick_and_place.py index 13e25828c..841099c2f 100644 --- a/examples/panda_pick_and_place.py +++ b/examples/panda_pick_and_place.py @@ -250,6 +250,9 @@ def move_fingers( world=world, position=[-0.1, 0, 1.0] ) +# Disable joint velocity limits +_ = [j.to_gazebo().set_velocity_limit(1_000) for j in panda.joints()] + # Enable contacts only for the finger links panda.get_link("panda_leftfinger").to_gazebo().enable_contact_detection(True) panda.get_link("panda_rightfinger").to_gazebo().enable_contact_detection(True)