From 5f1d88a89a14b0a0d978ac30fbc7f26b3d778f46 Mon Sep 17 00:00:00 2001 From: Dasun Gunasinghe Date: Thu, 23 Nov 2023 15:44:55 +1000 Subject: [PATCH] Reversion for testing back on real panda --- armer/robots/ROSRobot.py | 45 +++++++++++++++++++--------------------- armer/utils.py | 12 +++-------- 2 files changed, 24 insertions(+), 33 deletions(-) diff --git a/armer/robots/ROSRobot.py b/armer/robots/ROSRobot.py index 05d86ca..d47bbc2 100644 --- a/armer/robots/ROSRobot.py +++ b/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 diff --git a/armer/utils.py b/armer/utils.py index c125e1a..2691f4e 100644 --- a/armer/utils.py +++ b/armer/utils.py @@ -31,19 +31,13 @@ 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: @@ -51,7 +45,7 @@ def trapezoidal(robot: rtb.Robot, qf: np.ndarray, frequency=500, 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)