12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added comments to python example

release/4.3a0
Frank Dellaert 2019-04-15 15:19:06 -04:00
parent 94dcfdfe9f
commit 53980ae318
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cython/gtsam/examples/PlanarManipulatorExample.py
View File

@ -115,7 +115,7 @@ class ThreeLinkArm(object):
l1Zl1 = expmap(0.0, 0.0, q[0]) l1Zl1 = expmap(0.0, 0.0, q[0])
l2Zl2 = expmap(0.0, self.L1, q[1]) l2Zl2 = expmap(0.0, self.L1, q[1])
l3Zl3 = expmap(0.0, 7.0, q[2]) l3Zl3 = expmap(0.0, self.L1+self.L2, q[2])
return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0) return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0)
def ik(self, sTt_desired, e=1e-9): def ik(self, sTt_desired, e=1e-9):
@ -297,12 +297,18 @@ def run_example():
""" Use trajectory interpolation and then trajectory tracking a la Murray """ Use trajectory interpolation and then trajectory tracking a la Murray
to move a 3-link arm on a straight line. to move a 3-link arm on a straight line.
""" """
# Create arm
arm = ThreeLinkArm() arm = ThreeLinkArm()
# Get initial pose using forward kinematics
q = np.radians(vector3(30, -30, 45)) q = np.radians(vector3(30, -30, 45))
sTt_initial = arm.fk(q) sTt_initial = arm.fk(q)
# Create interpolated trajectory in task space to desired goal pose
sTt_goal = Pose2(2.4, 4.3, math.radians(0)) sTt_goal = Pose2(2.4, 4.3, math.radians(0))
poses = trajectory(sTt_initial, sTt_goal, 50) poses = trajectory(sTt_initial, sTt_goal, 50)
# Setup figure and plot initial pose
fignum = 0 fignum = 0
fig = plt.figure(fignum) fig = plt.figure(fignum)
axes = fig.gca() axes = fig.gca()
@ -310,6 +316,8 @@ def run_example():
axes.set_ylim(0, 10) axes.set_ylim(0, 10)
gtsam_plot.plot_pose2(fignum, arm.fk(q)) gtsam_plot.plot_pose2(fignum, arm.fk(q))
# For all poses in interpolated trajectory, calculate dq to move to next pose.
# We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose).
for pose in poses: for pose in poses:
sTt = arm.fk(q) sTt = arm.fk(q)
error = delta(sTt, pose) error = delta(sTt, pose)