Example node to move the Panda Arm

pick_place.py

#!/usr/bin/env python3

import sys
import rospy
import franka_gripper.msg
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg


def move_to_home(group):
    target = geometry_msgs.msg.Pose()
    home = {
        "orientation": {
            "x": -0.9182634002618467,
            "y": 0.39507715197555204,
            "z": -0.023010708525341525,
            "w": 0.013299587196596038,
        },
        "position": {
            "x": 0.5639108481080327,
            "y": -0.03735136685104907,
            "z": 0.6113676040355263,
        },
    }
    for attribute in home:
        for param in home[attribute]:
            setattr(getattr(target, attribute), param, home[attribute][param])
    group.set_pose_target(target)
    group.go(wait=True)


def grab_object_gripper(group):
    gripper_pos = {
        "width": 0.022,
        "epsilon": {
            "inner": 0.005,
            "outer": 0.005,
        },
        "speed": 0.1,
        "force": 5,
    }
    for attr in gripper_pos:
        if attr == 'epsilon':
            group.epsilon.inner = gripper_pos[attr]['inner']
            group.epsilon.outer = gripper_pos[attr]['outer']
        else:
            setattr(group, attr, gripper_pos[attr])


def main():
    rospy.init_node("pick_place", anonymous=True)

    # Initialize the MoveIt API
    moveit_commander.roscpp_initialize(sys.argv)
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    group = moveit_commander.MoveGroupCommander("panda_arm")

    # # Initialize the Gripper API
    # gripper = franka_gripper.Gripper()
    # gripper.set_epsilon(0.1)

    # Get the current gripper position and state
    current_gripper_pos = franka_gripper.msg.GraspGoal()

    print(current_gripper_pos)

    move_to_home(group)


if __name__ == "__main__":
    main()


"""
Home position state:

joints = [0.08928813181213652, 0.005930741249730712, -0.15777204922417817, -1.6100187916854638, 0.010822328315840827, 1.6681135690761775, 0.7433886230621072]

panda_link8 pose = [
header: 
  seq: 0
  stamp: 
    secs: 1681340522
    nsecs: 241985559
  frame_id: "panda_link0"
pose: 
  position: 
    x: 0.5639108481080327
    y: -0.03735136685104907
    z: 0.6113676040355263
  orientation: 
    x: -0.9182634002618467
    y: 0.39507715197555204
    z: -0.023010708525341525
    w: 0.013299587196596038 ]
panda_link8 RPY = [-3.0989539723913375, -0.031753440773068006, -0.8132900763316758]



Gripper State:

width: 0.0
epsilon: 
  inner: 0.0
  outer: 0.0
speed: 0.0
force: 0.0

"""