Reversion for testing back on real panda

armer/robots/ROSRobot.py

@@ -1089,36 +1089,33 @@ def pose_cb(self, goal: MoveToPoseGoal) -> None:
                 rospy.logwarn(f"IK solution within singularity threshold [{self.singularity_thresh}] -> ill-advised motion")
 
             # Generate trajectory from successful solution
-            # traj = self.traj_generator(self, qf=solution.q, max_speed=goal.speed if goal.speed else 0.2)
-            # Aiming for 500 steps (frequency) with a set max time (input from user)
-            time_array = np.linspace(0, 5, 500)
-            traj = self.traj_generator(self, qf=solution.q, move_time_sec=5, frequency=500, t_vec=time_array)
+            traj = self.traj_generator(self, qf=solution.q, max_speed=goal.speed if goal.speed else 0.2)
 
             # Construct a joint trajectory representation
             # TODO: test and implement this with joint trajectory controller
             print(f"traj time: {traj.t}")
             max_time = np.max(traj.t)
-            # time_array = np.linspace(0, max_time, len(traj.s))
-            # print(f"time array: {time_array}")
+            time_array = np.linspace(0, max_time, len(traj.s))
+            print(f"time array: {time_array}")
 
             # NOTE: adding one waypoint works, but adding all waypoints is an issue as time value is not correct per step
             jt_traj = JointTrajectory()
             jt_traj.header.stamp = rospy.Time.now()
             jt_traj.joint_names = list(self.joint_names)
             print(f"traj joint names: {jt_traj.joint_names}")
-            for idx in range(0,len(traj.s)):
-                print(f"current time array: {time_array[idx]} | rospy duration: {rospy.Duration(time_array[idx])}")
-                jt_traj_point = JointTrajectoryPoint()
-                jt_traj_point.time_from_start = rospy.Duration(traj.t[idx])
-                jt_traj_point.positions = list(traj.s[idx])
-                # jt_traj_point.velocities = list(traj.sd[idx])
-                jt_traj.points.append(jt_traj_point)
-
-            # jt_traj_point = JointTrajectoryPoint()
-            # jt_traj_point.time_from_start = rospy.Duration(np.max(traj.t))
-            # jt_traj_point.positions = list(traj.s[-1])
-            # #jt_traj_point.velocities = traj.sd[idx]
-            # jt_traj.points.append(jt_traj_point)
+            #for idx in range(0,len(traj.s)):
+            #    print(f"current time array: {time_array[idx]} | rospy duration: {rospy.Duration(time_array[idx])}")
+                #jt_traj_point = JointTrajectoryPoint()
+                #jt_traj_point.time_from_start = rospy.Duration(time_array[idx])
+                #jt_traj_point.positions = list(traj.s[idx])
+                #jt_traj_point.velocities = list(traj.sd[idx])
+                #jt_traj.points.append(jt_traj_point)
+
+            jt_traj_point = JointTrajectoryPoint()
+            jt_traj_point.time_from_start = rospy.Duration(np.max(traj.t))
+            jt_traj_point.positions = list(traj.s[-1])
+            #jt_traj_point.velocities = traj.sd[idx]
+            jt_traj.points.append(jt_traj_point)
 
             # Conduct 'Ghost' Robot Check for Collision throughout trajectory
             # NOTE: also publishes marker representation of trajectory for visual confirmation (Rviz)
@@ -1178,15 +1175,15 @@ def pose_cb(self, goal: MoveToPoseGoal) -> None:
                 #     rospy.sleep(0.01)
 
 
-                # # Send empty data at end to clear visual trajectory
-                # marker_traj = Marker()
-                # marker_traj.points = []
-                # self.display_traj_publisher.publish(marker_traj)
+                # Send empty data at end to clear visual trajectory
+                marker_traj = Marker()
+                marker_traj.points = []
+                self.display_traj_publisher.publish(marker_traj)
 
                 # if self.executor.is_succeeded():
                 self.pose_server.set_succeeded(MoveToPoseResult(success=True))
                 # else:
-                    # self.pose_server.set_aborted(MoveToPoseResult(success=False))
+                #     self.pose_server.set_aborted(MoveToPoseResult(success=False))
             else:
                 self.pose_server.set_aborted(MoveToPoseResult(success=False))
                 self.executor = None

armer/utils.py

@@ -31,27 +31,21 @@ def ikine(robot, target, q0, end):
 
     return type('obj', (object,), {'q' : np.array(result[0])})
 
-def trapezoidal(robot: rtb.Robot, qf: np.ndarray, frequency=500, move_time_sec: float=5, t_vec: list = []):
+def trapezoidal(robot: rtb.Robot, qf: np.ndarray, max_speed=None, frequency=500, move_time_sec: float=5):
     rospy.loginfo(f"TESTING Hotfix 96fd293")
 
     # ------------ NOTE: determine a joint trajectory (curved) based on time request
     # Solve a trapezoidal trajectory in joint space based on provided solution based on defined number of steps (t)
     # NOTE: this takes into account the robot's defined joint angle limits (defined in each robot's config as qlim_min and qlim_max)
-    if t_vec == []:
-        traj = rtb.mtraj(rtb.trapezoidal, q0=robot.q, qf=qf, t=frequency)
-    else:
-        traj = rtb.mtraj(rtb.trapezoidal, q0=robot.q, qf=qf, t=t_vec)
-
-    # print(f"traj t from trapezoidal function: {traj.t}")
-    # print(f"traj sd is: {traj.sd}")
+    traj = rtb.mtraj(rtb.trapezoidal, q0=robot.q, qf=qf, t=frequency)
 
     # Check if trajectory is valid
     if np.max(traj.qd) != 0 and move_time_sec != 0:
         # Scale the joint trajectories (in steps) to the overall expected move time (sec)
         scaled_qd = traj.qd*(frequency/move_time_sec)
         # print(f"max scaled qd: {np.max(scaled_qd)}")
         # return the generated trajectory scaled with expected speed/time
-        return Trajectory(name='trapezoidal-v1', t=traj.t, s=traj.q, sd=scaled_qd, sdd=None, istime=False)
+        return Trajectory(name='trapezoidal-v1', t=move_time_sec, s=traj.q, sd=scaled_qd, sdd=None, istime=True)
     else:
         rospy.logerr(f"Trajectory is invalid --> Cannot Solve. Given Move Time: [{move_time_sec}]")
         return Trajectory(name='invalid', t=1, s=robot.q, sd=None, sdd=None, istime=False)