Skip to content
Permalink
Browse files

add trajectory points when received points are few

  • Loading branch information...
aginika authored and IanTheEngineer committed Feb 14, 2015
1 parent 1a50ec7 commit b38ec257fece0711adb260ed6bc161096aa3ecae
Showing with 25 additions and 0 deletions.
  1. +25 −0 src/joint_trajectory_action/joint_trajectory_action.py
@@ -354,6 +354,31 @@ def _on_trajectory_action(self, goal):
control_rate = rospy.Rate(self._control_rate)

dimensions_dict = self._determine_dimensions(trajectory_points)

if num_points == 1:
# Add current position as trajectory point
first_trajectory_point = JointTrajectoryPoint()
first_trajectory_point.positions = self._get_current_position(joint_names)
if dimensions_dict['velocities']:
first_trajectory_point.velocities = [0.0] * len(joint_names)
if dimensions_dict['accelerations']:
first_trajectory_point.accelerations = [0.0] * len(joint_names)
first_trajectory_point.time_from_start = rospy.Duration(0)
trajectory_points.insert( 0, first_trajectory_point )
num_points = len(trajectory_points)

if num_points == 2:
# Add midpoint of 2 trajectory points
mid_trajectory_point = JointTrajectoryPoint()
mid_trajectory_point.positions = [(p1 + p2)/2 for (p1, p2) in zip(trajectory_points[0].positions, trajectory_points[1].positions)]
if dimensions_dict['velocities']:
mid_trajectory_point.velocities = [(v1 + v2)/2 for (v1, v2) in zip(trajectory_points[0].velocities, trajectory_points[1].velocities)]
if dimensions_dict['accelerations']:
mid_trajectory_point.accelerations = [(a1 + a2)/2 for (a1, a2) in zip(trajectory_points[0].accelerations, trajectory_points[1].accelerations)]
mid_trajectory_point.time_from_start = (trajectory_points[0].time_from_start + trajectory_points[1].time_from_start)/2
trajectory_points.insert( 1, mid_trajectory_point )
num_points = len(trajectory_points)

# Force Velocites/Accelerations to zero at the final timestep
# if they exist in the trajectory
# Remove this behavior if you are stringing together trajectories,

0 comments on commit b38ec25

Please sign in to comment.
You can’t perform that action at this time.