# How do you control the end effector of a rigid body system using keyboard keys?

So in my lab, I am to create a rigid body tree robot that has an end effector that can be teleoperated by keyboard commands. I found a function called waitforbuttonpress that allowed me to give the user the ability to select a specific keyboard key and then use that key to change the position vector to a different value. I placed the inverse kinematics solver inside of the function below because I believed that the function would solve the new angle joints based off of every time the position of the end effector changed, though for some reason I am getting some error in the inverse kinematics solver saying: Error using trvec2tform Expected t to be an array with number of columns equal to 3. I am using the home configuration of the robot as input for the kinematic solver function. Any advice is welcome below is the entire code of both the rigid body system as well as the kinematic solver function.

pbot = robotics.RigidBodyTree('DataFormat','column','MaxNumBodies',2);

body1 = robotics.RigidBody('body1');
joint1 = robotics.Joint('jnt1','revolute');
joint1.JointAxis = [1 1 1];
setFixedTransform(joint1,trvec2tform([0 0 2]));
body1.Joint = joint1;

s0 = homeConfiguration(pbot);
weights = [0, 0, 0, 1, 1, 1];
ik = robotics.InverseKinematics('RigidBodyTree',pbot);

sInitial = s0;

disp(body1)
keyboard_move(s0);

show(pbot)

function keyboard_move(x)
k = waitforbuttonpress;
p = get(gcf, 'CurrentCharacter');
while p ~= 'q'
if p == 'a'
x = x + [0 -.1 .1];
k = waitforbuttonpress;
p = get(gcf, 'CurrentCharacter');
elseif p == 's'
x = x + [-.1 0 .1];
k = waitforbuttonpress;
p = get(gcf, 'CurrentCharacter');
elseif p == 'd'
x = x + [0 .1 .1];
k = waitforbuttonpress;
p = get(gcf, 'CurrentCharacter');
elseif p == 'w'
x = x + [.1 0 .1];
k = waitforbuttonpress;
p = get(gcf, 'CurrentCharacter');
else
fprintf("Please enter a correct key: 'a', 's', 'd', 'w'")
end
mover = ik('body1',trvec2tform([0 0 1 + x]), weights, s0);
sInitial = mover;
end
end

end


for some reason I am getting some error in the inverse kinematics solver saying:

Error using trvec2tform Expected t to be an array with number of columns equal to 3.

Correct - you're not providing an array to trvec2tform with number of columns equal to 3. You're not using any punctuation (COMMAS!) in your vector definitions, which makes my eye twitch, and Matlab's also having a hard time with it.

It's not clear what x is, since it's an input to your function, but when you use it on other lines you've got it as x = x + [dx dy dz] (I'm assuming) However, when you use it as an input to trvec2tform, you're not doing x + [something], you're doing [something + x], all together in the same square brackets.

That is, you're including what appears to be a positive x, i.e., + x in the "fourth column" position, because you're using whitespace as your delimiter in your vector definition.

So, if your x vector is an accumulation of little steps, say x = [Dx Dy Dz], and you're trying to pass some initial positions [x0 y0 z0] plus your accumulation of steps, then what you are telling Matlab is:

[x0 y0 z0 + x]


Which Matlab is interpreting as:

[x0 y0 z0 +Dx +Dy +Dz]


where you appear to instead mean:

[x0 y0 z0] + x


which Matlab would the interpret as:

[x0 + Dx, y0 + Dy, z0 + Dz];


The line I wrote above would work fine because I included commas, so it's clear to Matlab where the columns are broken up, however even if you put commas in as-is, you would see that there's no clear way to interpret what you've written, i.e.:

[x0, y0, z0 + x]


would probably be interpreted as:

[x0, y0, z0 + Dx, z0 + Dy, z0 + Dz];


There's no way to include that x term inside the definition of your origin vector (which again is what I'm assuming the [0 0 1] is meant to be).