Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

add minjerk interpolation method #72

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

add minjerk interpolation method #72

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
bc76d5e
Merge pull request #71 from RethinkRobotics/release-1.2.0
Dec 30, 2015
d5c9988
add minjerk interpolation method
k-okada Jul 16, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 10 additions & 5 deletions scripts/joint_trajectory_action_server.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@
)


def start_server(limb, rate, mode):
def start_server(limb, rate, mode, interpolation):
print("Initializing node... ")
rospy.init_node("rsdk_%s_joint_trajectory_action_server%s" %
(mode, "" if limb == 'both' else "_" + limb,))
Expand All @@ -70,11 +70,11 @@ def start_server(limb, rate, mode):
jtas = []
if limb == 'both':
jtas.append(JointTrajectoryActionServer('right', dyn_cfg_srv,
rate, mode))
rate, mode, interpolation))
jtas.append(JointTrajectoryActionServer('left', dyn_cfg_srv,
rate, mode))
rate, mode, interpolation))
else:
jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode))
jtas.append(JointTrajectoryActionServer(limb, dyn_cfg_srv, rate, mode, interpolation))

def cleanup():
for j in jtas:
Expand Down Expand Up @@ -102,8 +102,13 @@ def main():
choices=['position_w_id', 'position', 'velocity'],
help="control mode for trajectory execution"
)
parser.add_argument(
"-i", "--interpolation", default='bezier',
choices=['bezier', 'minjerk'],
help="interpolation method for trajectory generation"
)
args = parser.parse_args(rospy.myargv()[1:])
start_server(args.limb, args.rate, args.mode)
start_server(args.limb, args.rate, args.mode, args.interpolation)


if __name__ == "__main__":
Expand Down
77 changes: 67 additions & 10 deletions src/joint_trajectory_action/joint_trajectory_action.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
import numpy as np

import bezier
import minjerk

import rospy

Expand All @@ -59,7 +60,7 @@

class JointTrajectoryActionServer(object):
def __init__(self, limb, reconfig_server, rate=100.0,
mode='position_w_id'):
mode='position_w_id', interpolation='bezier'):
self._dyn = reconfig_server
self._ns = 'robot/limb/' + limb
self._fjt_ns = self._ns + '/follow_joint_trajectory'
Expand All @@ -72,6 +73,7 @@ def __init__(self, limb, reconfig_server, rate=100.0,
self._limb = baxter_interface.Limb(limb)
self._enable = baxter_interface.RobotEnable()
self._name = limb
self._interpolation = interpolation
self._cuff = baxter_interface.DigitalIO('%s_lower_cuff' % (limb,))
self._cuff.state_changed.connect(self._cuff_cb)
# Verify joint control mode
Expand Down Expand Up @@ -332,6 +334,47 @@ def _compute_bezier_coeff(self, joint_names, trajectory_points, dimensions_dict)
b_matrix[jnt, :, :, :] = bezier.bezier_coefficients(traj_array, d_pts)
return b_matrix

def _get_minjerk_point(self, m_matrix, idx, t, cmd_time, dimensions_dict):
pnt = JointTrajectoryPoint()
pnt.time_from_start = rospy.Duration(cmd_time)
num_joints = m_matrix.shape[0]
pnt.positions = [0.0] * num_joints
if dimensions_dict['velocities']:
pnt.velocities = [0.0] * num_joints
if dimensions_dict['accelerations']:
pnt.accelerations = [0.0] * num_joints
for jnt in range(num_joints):
m_point = minjerk.minjerk_point(m_matrix[jnt, :, :, :], idx, t)
# Positions at specified time
pnt.positions[jnt] = m_point[0]
# Velocities at specified time
if dimensions_dict['velocities']:
pnt.velocities[jnt] = m_point[1]
# Accelerations at specified time
if dimensions_dict['accelerations']:
pnt.accelerations[jnt] = m_point[-1]
return pnt

def _compute_minjerk_coeff(self, joint_names, trajectory_points, point_duration, dimensions_dict):
# Compute Full Minimum Jerk Curve
num_joints = len(joint_names)
num_traj_pts = len(trajectory_points)
num_traj_dim = sum(dimensions_dict.values())
num_m_values = len(['a0', 'a1', 'a2', 'a3', 'a4', 'a5', 'tm'])
m_matrix = np.zeros(shape=(num_joints, num_traj_dim, num_traj_pts-1, num_m_values))
for jnt in xrange(num_joints):
traj_array = np.zeros(shape=(len(trajectory_points), num_traj_dim))
for idx, point in enumerate(trajectory_points):
current_point = list()
current_point.append(point.positions[jnt])
if dimensions_dict['velocities']:
current_point.append(point.velocities[jnt])
if dimensions_dict['accelerations']:
current_point.append(point.accelerations[jnt])
traj_array[idx, :] = current_point
m_matrix[jnt, :, :, :] = minjerk.minjerk_coefficients(traj_array, point_duration)
return m_matrix

def _determine_dimensions(self, trajectory_points):
# Determine dimensions supplied
position_flag = True
Expand Down Expand Up @@ -385,15 +428,24 @@ def _on_trajectory_action(self, goal):
if dimensions_dict['accelerations']:
trajectory_points[-1].accelerations = [0.0] * len(joint_names)

# Compute Full Bezier Curve Coefficients for all 7 joints
pnt_times = [pnt.time_from_start.to_sec() for pnt in trajectory_points]
try:
b_matrix = self._compute_bezier_coeff(joint_names,
trajectory_points,
dimensions_dict)
if self._interpolation == 'minjerk':
# Compute Full MinJerk Curve Coefficients for all 7 joints
point_duration = [pnt_times[i+1] - pnt_times[i] for i in range(len(pnt_times)-1)]
m_matrix = self._compute_minjerk_coeff(joint_names,
trajectory_points,
point_duration,
dimensions_dict)
else:
# Compute Full Bezier Curve Coefficients for all 7 joints
b_matrix = self._compute_bezier_coeff(joint_names,
trajectory_points,
dimensions_dict)
except Exception as ex:
rospy.logerr(("{0}: Failed to compute a Bezier trajectory for {1}"
" arm with error \"{2}: {3}\"").format(
rospy.logerr(("{0}: Failed to compute a {1} trajectory for {2}"
" arm with error \"{3}: {4}\"").format(
self._interpolation,
self._action_name,
self._name,
type(ex).__name__, ex))
Expand Down Expand Up @@ -429,9 +481,14 @@ def _on_trajectory_action(self, goal):
cmd_time = 0
t = 0

point = self._get_bezier_point(b_matrix, idx,
t, cmd_time,
dimensions_dict)
if self._interpolation == 'minjerk':
point = self._get_minjerk_point(m_matrix, idx,
t, cmd_time,
dimensions_dict)
else:
point = self._get_bezier_point(b_matrix, idx,
t, cmd_time,
dimensions_dict)

# Command Joint Position, Velocity, Acceleration
command_executed = self._command_joints(joint_names, point, start_time, dimensions_dict)
Expand Down
Loading