I have followed the official documentation to create a pick-and-place task with the Franka manipulator arm: pick and place.
As I understand it, the pick-and-place controller class utilizes RMPFlow under the hood. RMPFlow outputs the desired joint positions and joint velocities for the next time frame.
To gain a better understanding of the joint forces and torques, I utilized the get_applied_joint_efforts() method from the Franka class and printed out the Joint Efforts. However, the output I received was an array with all values set to zero.
Now, my question is: since the robot is simulating and picking up an object while moving from the initial position to the picking location, how is it possible to have zero joint efforts? Could you please assist me with this?
For reference, I am attaching the code that I have written.
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp({"headless": False})
from omni.isaac.core import World
from omni.isaac.franka import Franka
from omni.isaac.core.objects import DynamicCuboid
from omni.isaac.franka.controllers import PickPlaceController
import numpy as np
class FrankaSim:
"""A Isaac-Sim based franka panda physics simulation"""
def __init__(self):
self._init_sim()
def _init_sim(self):
"""Setup the scene for physics simulation of franka panda"""
self._world = World(
physics_dt= 1/60.0,
rendering_dt=1.0/60,
stage_units_in_meters=1,
physics_prim_path="/World/physicsScene",
)
# add ground plane to the scene
self._world.scene.add_default_ground_plane()
# add cube to the scene
self._cube = self._world.scene.add(
DynamicCuboid(
prim_path="/World/cube",
name="cube",
position=np.array([0.6, -0.2, 0.025]),
scale=np.array([0.05, 0.05, 0.05]),
color=np.array([0, 0, 1.0]),
)
)
# add franka robot to the scene
self._franka = self._world.scene.add(
Franka(
prim_path="/World/franka_panda",
name="franka_panda",
)
)
# Initialize the controller
self._controller = PickPlaceController(
name="pick_place_controller",
gripper=self._franka.gripper,
robot_articulation=self._franka,
events_dt = [0.008, 0.005, 1, 0.1, 0.05, 0.05, 0.0025, 1, 0.008, 0.08]
# [0.001, 0.005, 0.1, 0.1, 0.0025, 0.001, 0.0025, 1, 0.008, 0.001]
)
# hook the callback functions
self._world.add_physics_callback(
"physics_callback", self._physics_step_callback
)
def _physics_step_callback(self, step_size):
print("\n>>>>>>>>>>>>>>>>>>>>>>> $$ <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<")
print("Executing the event no: ", self._controller.get_current_event(), "|| current time is: ", self._world. current_time)
# get global poses
cube_position, _ = self._cube.get_world_pose()
goal_position = np.array([-0.3, -0.3, 0.025])
current_joint_positions = self._franka.get_joint_positions()
current_joint_velocities = self._franka.get_joint_velocities()
# print for info
print("current CUBE position:\n ", cube_position)
print("current JOINTS positions [in radians]:\n ", current_joint_positions)
print("current JOINTS velocities [in radians/sec]:\n ", current_joint_velocities)
print("JOint efforts: ", self._franka.get_applied_joint_efforts())
# get the actions for the next time frame
actions = self._controller.forward(
picking_position=cube_position,
placing_position=goal_position,
current_joint_positions=current_joint_positions,
)
# apply articulation action on franka joints
self._franka.apply_action(actions)
def simulate_physics(self):
# some information
print("""
A simple pick and place task, executed in 10 phases.
Each phase runs for 1 second
Dt of each phase/ event step is defined
- Phase 0: Move end_effector above the cube center at the 'end_effector_initial_height'. [dt = 0.008]
- Phase 1: Lower end_effector down to encircle the target cube [dt = 0.005]
- Phase 2: Wait for Robot's inertia to settle. []dt = 0.1]
- Phase 3: close grip. [dt = 0.1]
- Phase 4: Move end_effector up again, keeping the grip tight (lifting the block). [dt = 0.0.0025]
- Phase 5: Smoothly move the end_effector toward the goal xy, keeping the height constant. [dt = 0.001]
- Phase 6: Move end_effector vertically toward goal height at the 'end_effector_initial_height'. [dt = 0.0025]
- Phase 7: loosen the grip. [dt = 1]
- Phase 8: Move end_effector vertically up again at the 'end_effector_initial_height' [dt = 0.008]
- Phase 9: Move end_effector towards the old xy position. [dt = 0.008]
""")
# first reset the world to initialze the physics handles
print(">>>>>>>>>>>>>>>>>> Resetting the world >>>>>>>>>>>\n")
self._world.reset()
## After reset, physics handles will be enabled, means we
## can start interacting with articulation i.e robot joints and links
## Initially robot joints are in same positions as we defined in urdf file
print(">>>>>>>>>>>>>>>>>> Joint Default State >>>>>>>>>>>>\n", self._franka.get_joints_default_state().positions)
#set the joint positions of franka
print(">>>>>>>>>>>>>>>>>> Setting initial joint positions of Franka >>>>>>>>>>>\n")
new_joint_pos = [0 ,0, 0, -1.57, 0, 1.57, -0.7853,0.00545289, 0.00730873]
self._franka.set_joint_positions(positions = new_joint_pos)
print(">>>>>>>>>>>>>>>>>> Opening Gripper >>>>>>>>>>>\n")
# open the gripper
self._franka.gripper.set_joint_positions(
self._franka.gripper.joint_opened_positions
)
while 1:
self._world.step(render=True)
if self._controller.is_done():
break
print("\n>>>>>>>>>>>>>>>>>> Pick and Place Task Done <<<<<<<<<<<<<<<<<<<<<<<<<<<<")
if __name__ == "__main__":
obj = FrankaSim()
obj.simulate_physics()
simulation_app.close()