Skip to content

Commit

Permalink
Reversion for testing back on real panda
Browse files Browse the repository at this point in the history
  • Loading branch information
DasGuna committed Nov 23, 2023
1 parent cc844af commit 5f1d88a
Show file tree
Hide file tree
Showing 2 changed files with 24 additions and 33 deletions.
45 changes: 21 additions & 24 deletions armer/robots/ROSRobot.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand Down
12 changes: 3 additions & 9 deletions armer/utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down

0 comments on commit 5f1d88a

Please sign in to comment.