Merge branch 'future'

Author: jhavl

roboticstoolbox/examples/neo.py

@@ -25,25 +25,16 @@
 n = 7
 
 # Make two obstacles with velocities
-s0 = sg.Sphere(
-    radius=0.05,
-    base=sm.SE3(0.52, 0.4, 0.3)
-)
+s0 = sg.Sphere(radius=0.05, base=sm.SE3(0.52, 0.4, 0.3))
 s0.v = [0, -0.2, 0, 0, 0, 0]
 
-s1 = sg.Sphere(
-    radius=0.05,
-    base=sm.SE3(0.1, 0.35, 0.65)
-)
+s1 = sg.Sphere(radius=0.05, base=sm.SE3(0.1, 0.35, 0.65))
 s1.v = [0, -0.2, 0, 0, 0, 0]
 
 collisions = [s0, s1]
 
 # Make a target
-target = sg.Sphere(
-    radius=0.02,
-    base=sm.SE3(0.6, -0.2, 0.0)
-)
+target = sg.Sphere(radius=0.02, base=sm.SE3(0.6, -0.2, 0.0))
 
 # Add the Panda and shapes to the simulator
 env.add(panda)
@@ -54,7 +45,7 @@
 # Set the desired end-effector pose to the location of target
 Tep = panda.fkine(panda.q)
 Tep.A[:3, 3] = target.base.t
-Tep.A[2, 3] += 0.1
+# Tep.A[2, 3] += 0.1
 
 
 def step():
@@ -69,7 +60,7 @@ def step():
 
     # Calulate the required end-effector spatial velocity for the robot
     # to approach the goal. Gain is set to 1.0
-    v, arrived = rtb.p_servo(Te, Tep, 0.5, 0.05)
+    v, arrived = rtb.p_servo(Te, Tep, 0.5, 0.01)
 
     # Gain term (lambda) for control minimisation
     Y = 0.01
@@ -108,9 +99,14 @@ def step():
         # Form the velocity damper inequality contraint for each collision
         # object on the robot to the collision in the scene
         c_Ain, c_bin = panda.link_collision_damper(
-            collision, panda.q[:n], 0.3, 0.05, 1.0,
-            start=panda.link_dict['panda_link1'],
-            end=panda.link_dict['panda_hand'])
+            collision,
+            panda.q[:n],
+            0.3,
+            0.05,
+            1.0,
+            start=panda.link_dict["panda_link1"],
+            end=panda.link_dict["panda_hand"],
+        )
 
         # If there are any parts of the robot within the influence distance
         # to the collision in the scene
@@ -147,5 +143,4 @@ def run():
 
 
 step()
-
-cProfile.run('run()')
+run()