ME 5(b): Homework #2 Solution Problem : Problem 2 (d,e), Chapter 3 of MLS. Let s review some material from the parts (b) of this problem: Part (b): Using the Product of Exponentials approach, the structure equations take the form: e ξ θ e ξ 2(d W ) e ξ 3θ 3 g BT () = e ξ 2θ 2 e ξ 22(d 2 W ) e ξ 23θ 23 g BT () = e ξ 3θ 3 e ξ 32(d 3 d 3, ) e ξ 33θ 33 g BT () where d 3, is the length of the third prismatic joint in the home position, which is d 3, = h2 + W 2. With this home configuration, g BT () = I [ W T ]. In the home position (the one showed in the left hand diagram of the figure which goes along with problem 3.2 in the MLS text) the twists are: ξ = ; ξ 2 = ; ξ 3 = w ξ 2 = ; ξ 22 = ; ξ 23 = w () w ξ 3 = ; ξ 32 = w h ; ξ 2 +b 2 33 = w Part (d): Using the results of part (b), the spatial Jacobians are simply: J s BT, θ = JBT,2 s θ 2 = JBT,3 s θ 3, or [ θ ξ ξ 2 ξ 3] d = [ ] θ 2 ξ 2 ξ 22 ξ 23 d 2 = [ ] θ 3 ξ 3 ξ 32 ξ 33 d 3 θ 3 θ 23 θ 33 where ξ ij is the appropriately transformed twist coordinates of the twists found in part (b) of the problem. In the case of this simple mechanism, the transformed twists can be determined
by inspection: cos θ (w + d ) sin θ + ξ = ; ξ 2 = sin θ ; ξ 3 = (w + d ) cos θ cos θ 2 (w + d 2 ) sin θ 2 ξ 2 = ; ξ 22 = sin θ 2 ; ξ 23 = (w + d 2 ) cos θ 2 cos(θ 32 + θ32) o ξ 3 = ; ξ 32 = sin(θ 32 + θ32) o ; ξ 33 = ξ 3 (2) where θ 32 is the angle made the d 3 with respect to the horizontal in the home position: θ o 32 = tan ( h W ). Part (e): The set of possible motions of the tool frame lies in the intersection of the range spaces of the three Jacobians, JBT, s, J BT,2 s, J BT,3 s. Thus, when any one of these Jacobians loses rank, the mechanism must be in an actuator singularity. Taking the determinants of the above equations, singular configurations exist when: d + W =, d 2 + W =, d 3 + d 3, =. The first condition corresponds to the first prismatic actuator collapsing so that revolute joints (, ) and (, 3) are collinear. The other conditions are similar. With regards to the actuator singularities, we need to find under what conditions the span of the wrenches generated by the actuators loses rank. We begin by noting that to an observer in the stationary frame, the wrench due to each prismatic joint will take the form: [ ] vi W i = S i (3) ρ i v i where S i is the strength of the i th primatic joint s force, v i is the direction in which the primatic actuator s force acts, and ρ i is a vector from the origin of the observing frame to a point on the line of action of the prismatic actuator. Now applying the above expression to each chain, we get the following wrenches: W = f cos θ sin θ ; W 2 = cos θ 2 f 2 sin θ 2 ; W 3 = cos θ 3 f 3 sin θ h cos θ 2 h cos θ 3 (4) 2 2 h cos 2 θ 3 2
where θ 3 = θ 3 +θ o 3. Note that when θ 2 = θ 3, the third wrench becomes linearly dependent upon the second wrench, and therefore the span of actuator wrenches loses rank, indicating that the mechanism is in an actuator singularity. Problem 2: Part (a): You can find the forward kinematics of this manipulator by using Denavit Hartenberg paramaters, or by product of exponentials. Here we use the D-H approach cosθ sinθ cosθ 2 sinθ 2 l g = sinθ cosθ g 2 = sinθ 2 cosθ 2 cosθ 3 sinθ 3 l 2 l 3 g 23 = sinθ 3 cosθ 3 g 34 = Plugging in the g values,: g ST = g g 2 g 23 g 34 (5) cos(θ + θ 2 + θ 3 ) sin(θ + θ 2 + θ 3 ) l cos(θ ) + l 2 cos(θ + θ 2 ) + l 3 cos(θ + θ 2 + θ 3 ) g ST = sin(θ + θ 2 + θ 3 ) cos(θ + θ 2 + θ 3 ) l sin(θ ) + l 2 sin(θ + θ 2 ) + l 3 sin(θ + θ 2 + θ 3 ) Thus, cos(θ + θ 2 + θ 3 ) sin(θ + θ 2 + θ 3 ) ) R ST = sin(θ + θ 2 + θ 3 ) cos(θ + θ 2 + θ 3 ) ) l cos(θ ) + l 2 cos(θ + θ 2 ) + l 3 cos(θ + θ 2 + θ 3 ) p ST = l sin(θ ) + l 2 sin(θ + θ 2 ) + l 3 sin(θ + θ 2 + θ 3 ) Part (b): Because we are only interested in the location of the tool frame origin, the Hybrid Jacobian can be simply found as: J H = [ ] dp dθ Now using the solutions from part (a): [ J H sinθ l = sin(θ + θ 2 )l 2 sin(θ + θ 2 + θ 3 )l 3 sin(θ + θ 2 )l 2 sin(θ + θ 2 + θ 3 )l 3 sin(θ + θ 2 + θ 3 ) cosθ l cos(θ + θ 2 )l 2 cos(θ + θ 2 + θ 3 )l 3 cos(θ + θ 2 )l 2 + cos(θ + θ 2 + θ 3 )l 3 cos(θ + θ 2 + θ 3 )l 3 3
2..8.6 2 3 4.4 -.2-2.4.6.8..2 Figure : Left: joint angles of manipulator as a function of time. Right: calculated path of end-effector. Part (c): Now we can use the fact that all of the link lengths are. Thus, [ ] J H sinθ sin(θ = + θ 2 ) sin(θ + θ 2 + θ 3 ) sin(θ + θ 2 ) sin(θ + θ 2 + θ 3 ) sin(θ + θ 2 + θ 3 ) cosθ cos(θ + θ 2 ) cos(θ + θ 2 + θ 3 ) cos(θ + θ 2 ) + cos(θ + θ 2 + θ 3 ) cos(θ + θ 2 + θ 3 ) We are given that [ ] x(t) = y(t) [ ].5.5cos(ωt)..5sin(ωt) where ω controls the speed of the trajectory. The value of ω is left up to you, with the suggestion of π or π 2. You are asked to simulate the robot following a circular trajectory using the resolved rate trajectory planning scheme with a simple pseudo inverse solution: Θ = J θṗ (6) Remember that the pseudo-inverse can be found with the following equation: J = J T (J T ) (7) The actual coding of this approach will depend on whether you are using Mathematica, Matlab, or another package. The attached code provides an example in Mathematica. Also, the exact trajectory of each simulated joint will also depend upon your initial conditions. Figure shows the output from the attached code. 4
(*****************************************************************) (** Mathematica program to simulate redundancy-resolution **) (** based on the Jacobian pseudo-inverse **) (*****************************************************************) (* Link Lengths of the 3R robot *) l =.; l2 =.; l3 =. (* forward kinematics functions *) x[t_,t2_,t3_]:= l Cos[t] + l2 Cos[t+t2] + l3 Cos[t+t2+t3] y[t_,t2_,t3_]:= l Sin[t] + l2 Sin[t+t2] + l3 Sin[t+t2+t3] (* the Jacobian matrix *) JacMat[t_,t2_,t3_]:=Block[{J,J2,J3,J2,J22,J23,s,c,s2,c2,s23,c 23}, s = Sin[t]; s2 = Sin[t+t2]; s23 = Sin[t+t2+t3]; c = Cos[t]; c2 = Cos[t+t2]; c23 = Cos[t+t2+t3]; J3 = - l3 s23; J2 = J3 - (l2 s2); J = J2 - (l s); J23 = l3 c23; J22 = J23 + l2 c2; J2 = J22 + l c; Return[{{J, J2, J3},{J2,J22,J23}}]; ] (* returns the Jacobian pseudo-inverse *) JPseudo[t_,t2_,t3_]:= Block[{jac}, jac = JacMat[t,t2,t3]; Return[PseudoInverse[jac]]; ] (* equations for desired circular path *) R=.5 (* radius of circle *) OMEGA = Pi/2 (* speed along path *) xd[t_]:=.5 - R Sin[OMEGA t] (* desired x-position *) yd[t_]:= - R Cos[OMEGA t] (* desired y-position *) xddot[t_]:= -R OMEGA Cos[OMEGA t] (* desired x velocity *) yddot[t_]:= R OMEGA Sin[OMEGA t] (* desired y velocity *) pdot[t_]:= List[xddot[t],yddot[t]] (* constant basis vectors *) XVECT = List[,,]; YVECT = List[,,]; ZVECT = List[,,] (* use Mathematica's NDSolve capability to integrate the redundancy resolution differential equations. The manipulator is initialized at configuration (97.88,-97.88,-97.88) degrees. This function returns a vector of interpolating functions *) Angles=NDSolve[{ t'[t] == XVECT. (JPseudo[t[t],t2[t],t3[t]]. pdot[t]), t2'[t] == YVECT. (JPseudo[t[t],t2[t],t3[t]]. pdot[t]), t3'[t] == ZVECT. (JPseudo[t[t],t2[t],t3[t]]. pdot[t]), t[] == 97.88*(Pi/8), t2[]== -97.88*(Pi/8), t3[]== - 97.88*(Pi/8) }, {t,t2,t3},{t,,4.5}] (* plot the joint angles resulting from the NDSolve integration *) PlotAngles = Plot[{Evaluate[t[t] /. Angles], Evaluate[t2[t] /. Angles],
Evaluate[t3[t] /. Angles]}, {t,,4.4}] (* Plot the associated end-effector coordinates *) PlotXY = ParametricPlot[ {x[evaluate[t[t] /. Angles][[]], Evaluate[t2[t] /. Angles][[]], Evaluate[t3[t] /. Angles][[]] ], y[evaluate[t[t] /. Angles][[]], Evaluate[t2[t] /. Angles][[]], Evaluate[t3[t] /. Angles][[]] ] }, {t,.,4.4}, AspectRatio->.]