Clean up

dfki-ric · Nov 18, 2023 · 551f30b · 551f30b
1 parent f32a36a
commit 551f30b
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 10 deletions.
diff --git a/movement_primitives/dmp/_dual_cartesian_dmp.py b/movement_primitives/dmp/_dual_cartesian_dmp.py
@@ -124,14 +124,12 @@ def dmp_step_dual_cartesian_python(
         current_ydd[pvs] = (
             alpha_y * (
                 beta_y * (goal_y[pps] - current_y[pps])
-                + execution_time * goal_yd[pvs]
-                - execution_time * current_yd[pvs]
+                + execution_time * (goal_yd[pvs] - current_yd[pvs])
                 - smoothing
             )
-            + goal_ydd[pvs] * execution_time ** 2
             + f[pvs]
             + cdd[pvs]
-        ) / execution_time ** 2
+        ) / execution_time ** 2 + goal_ydd[pvs]
         current_yd[pvs] += dt * current_ydd[pvs] + cd[pvs] / execution_time
         current_y[pps] += dt * current_yd[pvs]
 
@@ -148,14 +146,12 @@ def dmp_step_dual_cartesian_python(
                 alpha_y * (
                     beta_y * pr.compact_axis_angle_from_quaternion(pr.concatenate_quaternions(
                         goal_y[ops], pr.q_conj(current_y[ops])))
-                    + execution_time * goal_yd[ovs]
-                    - execution_time * current_yd[ovs]
+                    + execution_time * (goal_yd[ovs] - current_yd[ovs])
                     - smoothing
                 )
-                + goal_ydd[ovs] * execution_time ** 2
                 + f[ovs]
                 + cdd[ovs]
-            ) / execution_time ** 2
+            ) / execution_time ** 2 + goal_ydd[ovs]
             current_yd[ovs] += dt * current_ydd[ovs] + cd[ovs] / execution_time
             current_y[ops] = pr.concatenate_quaternions(
                 pr.quaternion_from_compact_axis_angle(dt * current_yd[ovs]), current_y[ops])

diff --git a/movement_primitives/dmp_fast.pyx b/movement_primitives/dmp_fast.pyx
@@ -672,8 +672,7 @@ cpdef dmp_step_dual_cartesian(
             current_ydd[ovs] = (
                 alpha_y * (
                     beta_y * compact_axis_angle_from_quaternion(concatenate_quaternions(goal_y[ops], q_conj(current_y[ops])))
-                    + execution_time * goal_yd[ovs]
-                    - execution_time * current_yd[ovs]
+                    + execution_time * (goal_yd[ovs] - current_yd[ovs])
                     - smoothing_orn
                 )
                 + f[ovs]