Re: Kinematic factory

On Thu, 15 Jun 2006, sailendra sethi wrote:

> i defined a new robot type in Kinematic factory.hpp as :
>
> if (name == "MY_ARM")
>   {
>     SerialZXXDWH* ki = new SerialZXXDWH();
> 	ki->geometrySet(3.2,4.70,4.70,0.0,0.0,0.2);
> 	ki->offsetSet(0.);
>       ki->eccentricitySet(0.);
>     return ki;
>   }
>
> i am taking link lengths in inches.
>
> How it works when i give a new robo type. Is there any  function so that i 
> can analyse the calculation for this ZXXDWH type configuration. It is not 
> moving to given point may be either due to motor constraints. Please give me 
> some way to solve this problem.
>
It is very unlikely that the SerialZXXDWH kinematics suits your needs,
because it involves an (old and patented) wrist design by KUKA...

But what do you mean exactly with "any function so that I can analyse the
calculation"?

I include a matlab function that calculates the forward position kinematics
of a six revolute joint robot (fpk321); maybe you can use it for your
analysis...?

Herman
==================

function [R,p]=fpk321(l,q)
% [R,p]=fpk321(l,q)
%
% Calculates the forward position kinematics of a serial robot arm
% with six revolute joints in a `321' architecture
% (Featherstone, R., I. J. Rob. Research, 1984).
% `l' is a 4-vector of  link lengths (links 1, 2, 3 and 6 only, since
% joints 4, 5 and 6 intersect each other in the same point).
% `q' is the 6-vector of joint values; joints 1, 4 and 6 are positive
% along the positive direction of their Z-axes (see below); joints
% 2, 3 and 5 rotate positively around ther negative X-axes.
% The function returns the rotation matrix `R' and the position vector
% `p' of the end effector frame with respect to the base frame of the
% robot. Both frames are parallel if the robot is in its fully upright
% position. Their Z-axes then point upward, their Y-axes forward and their
% X-axes to the right.
%

% Herman.Bruyninckx[@]mech.kuleuven.be    12 JUL 1998

% link lengths:
l1 = l(1); l2 = l(2); l3 = l(3); l6 = l(4);

c1 = cos(q(1));       s1  = sin(q(1));
c23= cos(q(2)+q(3));  s23 = sin(q(2)+q(3));
c4 = cos(q(4));       s4  = sin(q(4));
c5 = cos(q(5));       s5  = sin(q(5));
c6 = cos(q(6));       s6  = sin(q(6));
c4s5 = c4 * s5;      s4s5 = s4 * s5;
s5c6 = s5 * c6;      s5s6 = s5 * s6;
c1c23  = c1 * c23; c1s23  = c1 * s23;
s1c23  = s1 * c23; s1s23  = s1 * s23;

R(2,1) = s4*c5*s6 - c4*c6;
R(3,1) = c4*c5*s6 + s4*c6;
R(1,1) = -s1c23*R(3,1) - c1*R(2,1) + s1s23*s5s6;
R(2,1) =  c1c23*R(3,1) - s1*R(2,1) - c1s23*s5s6;
R(3,1) =   -s23*R(3,1) - c23*s5s6;

R(2,2) = s4*c5*c6 + c4*s6;
R(3,2) = c4*c5*c6 - s4*s6;
R(1,2) = -s1c23*R(3,2) - c1*R(2,2) + s1s23*s5c6;
R(2,2) =  c1c23*R(3,2) - s1*R(2,2) - c1s23*s5c6;
R(3,2) =   -s23*R(3,2) - c23*s5c6;

R(1,3) = -s1c23*c4s5 - c1*s4s5 - s1s23*c5;
R(2,3) =  c1c23*c4s5 - s1*s4s5 + c1s23*c5;
R(3,3) =   -s23*c4s5 + c23*c5;

% vertical distance of wrist from shoulder:
dWv = cos(q(2)) * l2 + c23 * l3;
% horizontal distance of wrist from shoulder:
dWh = sin(q(2)) * l2 + s23 * l3;

% Position of wrist: */
Pwx = -s1 * dWh;
Pwy =  c1 * dWh;
Pwz =  l1 + dWv;

% End effector position = position of last link + position of wrist:
p = [R(1,3) * l6 + Pwx; R(2,3) * l6 + Pwy; R(3,3) * l6 + Pwz];


_______________________________________________
Orocos mailing list
[email protected]
http://lists.mech.kuleuven.be/mailman/listinfo/orocos

Disclaimer: http://www.kuleuven.be/cwis/email_disclaimer.htm