update neo example
@@ -25,25 +25,16 @@ panda.q = panda.qr
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 @@ env.add(target)
# 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()
