added first example for python bindings

bindings/python/examples/panda_ik.py

@@ -0,0 +1,111 @@
+import os
+
+import rospkg
+from xbot2_interface import pyxbot2_interface as xbi
+from pyopensot.tasks.velocity import Postural, Cartesian, Manipulability, MinimumEffort
+from pyopensot.constraints.velocity import JointLimits, VelocityLimits
+import pyopensot as pysot
+import numpy as np
+import rospy
+from sensor_msgs.msg import JointState
+import subprocess
+
+# Check for franka_cartesio_condif package
+try:
+    package_path = rospkg.RosPack().get_path('franka_cartesio_config')
+except:
+    print("To run this example is needed the franka_cartesio_config package that can be download here: https://github.com/EnricoMingo/franka_cartesio_config")
+
+# roslaunch the panda.launch with the robot description and rviz
+launch_path = package_path + "/launch/panda.launch"
+roslaunch = subprocess.Popen(['roslaunch', launch_path], stdout=subprocess.PIPE, shell=False)
+rviz_file_path = os.getcwd() + "/panda_ik.rviz"
+rviz = subprocess.Popen(['rviz',  '-d', f'{rviz_file_path}'], stdout=subprocess.PIPE, shell=False)
+
+# Initiliaze node and wait for robot_description parameter
+rospy.init_node("panda_ik", disable_signals=True)
+while not rospy.has_param('/robot_description'):
+    pass
+
+# Get robot description parameter and initialize model interface (with Pinocchio)
+urdf = rospy.get_param('/robot_description')
+model = xbi.ModelInterface2(urdf)
+
+# Set a homing configuration
+q = [0., -0.7, 0., -2.1, 0., 1.4, 0.]
+model.setJointPosition(q)
+model.update()
+
+# Get Joint Limits and Velocity Limits, define dt
+qmin, qmax = model.getJointLimits()
+qlims = JointLimits(model, qmax, qmin)
+
+dqmax = model.getVelocityLimits()
+dt = 1./100.
+dqlims = VelocityLimits(model, dqmax, dt)
+
+# Create postural task (it is created at the q configuration previously set
+p = Postural(model)
+
+# Create a Cartesian task at frame panda_link7, set lambda gain
+c = Cartesian("Cartesian", model, "panda_link8", "world")
+c.setLambda(0.1)
+
+# Retrieve actual pose of the frame to be used as reference
+ref = c.getActualPose().copy()
+
+# Create the stack:
+# 1st priority Cartesian position
+# 2nd priority Cartesian orientation
+# 3rd priority postural
+s = ( (c%[0,1,2]) / (c%[3,4,5]) / p) << qlims
+#s<<dqlims
+s.update()
+
+# Get the actual reference for postural and Cartesian task
+qref, dqref = p.getReference()
+print(f"qref: {qref}")
+print(f"dqref: {dqref}")
+pose_ref, vel_ref = c.getReference()
+print(f"pose_ref: {pose_ref}")
+print(f"vel_ref: {vel_ref}")
+
+# Creates iHQP solver with stack (using qpOASES as backend)
+solver = pysot.iHQP(s)
+
+# IK loop
+rate = rospy.Rate(1./dt)
+pub = rospy.Publisher('joint_states', JointState, queue_size=10)
+msg = JointState()
+msg.name = model.getJointNames()
+t = 0.
+alpha = 0.01
+while not rospy.is_shutdown():
+    # Update actual position in the model
+    model.setJointPosition(q)
+    model.update()
+
+    # Compute new reference for Cartesian task
+    pose_ref.translation[0] += alpha * np.sin(2.*3.1415 * t)
+    pose_ref.translation[1] += alpha * np.cos(2.*3.1415 * t)
+    t = t + alpha
+    if t > 1.:
+        t = 0
+    c.setReference(pose_ref)
+
+    # Update Stack
+    s.update()
+
+    # Solve
+    dq = solver.solve()
+    q += dq # Is fixed base so we can siply sum the result
+
+    # Publish joint states
+    msg.position = q
+    msg.header.stamp = rospy.get_rostime()
+    pub.publish(msg)
+
+    rate.sleep()
+
+roslaunch.kill()
+rviz.kill()

bindings/python/examples/panda_ik.rviz

@@ -0,0 +1,185 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /RobotModel1/Links1
+        - /RobotModel1/Links1/panda_link81
+      Splitter Ratio: 0.5
+    Tree Height: 549
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 100
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        panda_link0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: true
+          Value: true
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 3.1808619499206543
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: -0.005027552600950003
+        Y: -0.32176125049591064
+        Z: 0.3776461184024811
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.4003981947898865
+      Target Frame: <Fixed Frame>
+      Yaw: 0.5853981971740723
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1200
+  X: 72
+  Y: 27