PDA

View Full Version : Feeding Coordinates to Pincher Arm.



EpicZa
04-23-2015, 06:38 AM
Hi,
I'm a beginner to arduino and arm programming, but have decent experience in c++, I currently have a project in c++ in visual studio that outputs (x,y,z) coords, right hand rule (openGL for reference). (I can write them to file if needs be).

Ive added the arduino visual studio add on to VS, so I can program my board and my arm. But I can't find much info on moving the pincher head directly to a point in free space without assigning values to each servo.

I'd basically like to set the base of the arm as (0,0,0), set an exclusion zone, and then be able to send the pincher head to any coordinate I send to the code, within the given range. Is this possible?

I realise this is going to more complex than I'd like, but I think I've missed some documentation.

Can anyone point in the direction of tutorials / an API reference that would get me going?

Thanks

kgranat
04-23-2015, 10:59 AM
We have some firmware for the pincher that will allow you to send the Pincher Cartesian, cylindrical, or joint data. You can find info on the firmware and a demo application here
http://learn.trossenrobotics.com/20-interbotix/robot-arms/137-interbotix-arm-link-software.html
I'd recommend running the demo app to get acquainted with the functionality.

The program sends the coordinates via a serial packet. This article goes over the packet structure and other info
http://learn.trossenrobotics.com/arbotix/arbotix-communication-controllers/31-arm-link-reference.html
You'll need to generate the correct serial pacets to get the arm to move to different positions. We have example packets here
http://learn.trossenrobotics.com/20-interbotix/robot-arms/142-arm-link-packet-examples.html

You can find all of the documetnation links for the pincher here
http://learn.trossenrobotics.com/38-interbotix-robots/122-phantomx-pincher-robot-arm.html

jwatte
04-23-2015, 05:23 PM
Kyles suggestions give you the actual software to do what you want.

Separately, you may want to read up on "two-bone IK" which is a simple way of solving the question "I have two linked arms of length A and B; how can I get them to end up at point X,Y" in 2D, and then you add in a simple atan2() call to figure out which orientation to point the base servo. This is the math that goes behind the libraries Kyle links to.

EpicZa
04-24-2015, 04:25 AM
We have some firmware for the pincher that will allow you to send the Pincher Cartesian, cylindrical, or joint data. You can find info on the firmware and a demo application here
http://learn.trossenrobotics.com/20-interbotix/robot-arms/137-interbotix-arm-link-software.html
I'd recommend running the demo app to get acquainted with the functionality.

The program sends the coordinates via a serial packet. This article goes over the packet structure and other info
http://learn.trossenrobotics.com/arbotix/arbotix-communication-controllers/31-arm-link-reference.html
You'll need to generate the correct serial pacets to get the arm to move to different positions. We have example packets here
http://learn.trossenrobotics.com/20-interbotix/robot-arms/142-arm-link-packet-examples.html

You can find all of the documetnation links for the pincher here
http://learn.trossenrobotics.com/38-interbotix-robots/122-phantomx-pincher-robot-arm.html

Yes I actually downloaded the arm link software and was messing around with it, I realised after you posted though that I also have the code for the ArmLink so I think I can probably cut it up to what I need.

Those second two links are great, I think thats what I was missing, those along with the armlink source should set me in the right direction thanks!

EpicZa
04-24-2015, 04:28 AM
Kyles suggestions give you the actual software to do what you want.

Separately, you may want to read up on "two-bone IK" which is a simple way of solving the question "I have two linked arms of length A and B; how can I get them to end up at point X,Y" in 2D, and then you add in a simple atan2() call to figure out which orientation to point the base servo. This is the math that goes behind the libraries Kyle links to.

Very helpful thank you, I'm trying to get the robot to follow my finger tips, this should help me understand whats going on when I'm sending packets/coords.

EpicZa
04-27-2015, 07:03 AM
I think I'm making some progress, been looking around at the examples and the armlink java code, but for some reason this doesn't work! It simply outputs ÿddd }U and nothing moves? Any idea where I'm going wrong? I tried using if(armlink.ReadMsgs()), but calling that it won't even enter the loop.



#include <ax12.h>
#include <BioloidController.h>
#include <ArmLink\ArmLink.h>

BioloidController bioloid = BioloidController(1000000);

ArmLink armlink= ArmLink();

void setup(){
Serial.begin(38400);
Serial.println("Please wait 10 seconds.");
delay(10000);
Serial.println("Send Data.");

}

void loop(){
if (Serial.available()){
int inByte = Serial.read();
switch (inByte){
case '1':
Serial.println("Sending Packet...");
sendCommanderPacket(100, 100, 100, 0, 0, 256, 125, 0, 0);
break;
}
}
}

void sendCommanderPacket(int x, int y, int z, int wristAngle, int wristRotate, int gripper, int delta, int button, int extended)
{
//clear the serial port for the next round of communications
Serial.flush();
byte xValBytes[2];
xValBytes[0] = intToBytesLOW(x);
xValBytes[1] = intToBytesHIGH(x);


byte yValBytes[2];
yValBytes[0] = intToBytesLOW(y);
yValBytes[1] = intToBytesHIGH(y);


byte zValBytes[2];
zValBytes[0] = intToBytesLOW(z);
zValBytes[1] = intToBytesHIGH(z);


byte wristRotValBytes[2];
wristRotValBytes[0] = intToBytesLOW(wristRotate);
wristRotValBytes[1] = intToBytesHIGH(wristRotate);


byte wristAngleValBytes[2];
wristAngleValBytes[0] = intToBytesLOW(wristAngle);
wristAngleValBytes[1] = intToBytesHIGH(wristAngle);


byte gripperValBytes[2];
gripperValBytes[0] = intToBytesLOW(gripper);
gripperValBytes[1] = intToBytesHIGH(gripper);


//cast int to bytes
byte buttonByte = byte(button);
byte extValByte = byte(extended);
byte deltaValByte = byte(delta);
boolean flag = true;
//calculate checksum - add all values, take lower byte (%256) and invert result (~). you can also invert results by (255-sum)
byte checksum = (byte)(~(xValBytes[1] + xValBytes[0] + yValBytes[1] + yValBytes[0] + zValBytes[1] + zValBytes[0] + wristAngleValBytes[1] + wristAngleValBytes[0] + wristRotValBytes[1] + wristRotValBytes[0] + gripperValBytes[1] + gripperValBytes[0] + deltaValByte + buttonByte + extValByte) % 256);

//send commander style packet. Following labels are for cartesian mode, see function comments for clyindrical/backhoe mode
//try to write the first header byte
//Serial.write(0xff);
Serial.write(0xff);//header
Serial.write(xValBytes[1]); //X Coord High Byte
Serial.write(xValBytes[0]); //X Coord Low Byte
Serial.write(yValBytes[1]); //Y Coord High Byte
Serial.write(yValBytes[0]); //Y Coord Low Byte
Serial.write(zValBytes[1]); //Z Coord High Byte
Serial.write(zValBytes[0]); //Z Coord Low Byte
Serial.write(wristAngleValBytes[1]); //Wrist Angle High Byte
Serial.write(wristAngleValBytes[0]); //Wrist Angle Low Byte
Serial.write(wristRotValBytes[1]); //Wrist Rotate High Byte
Serial.write(wristRotValBytes[0]); //Wrist Rotate Low Byte
Serial.write(gripperValBytes[1]); //Gripper High Byte
Serial.write(gripperValBytes[0]); //Gripper Low Byte
Serial.write(deltaValByte); //Delta Low Byte
Serial.write(buttonByte); //Button byte
Serial.write(extValByte); //Extended instruction
Serial.write(checksum); //checksum
}


byte intToBytesLOW(int convertInt)
{
byte returnBytes; // array that holds the two bytes to return
byte mask = byte(255); //mask for the low byte (255/0xff)
returnBytes = byte(convertInt & mask);//low byte - perform an '&' operation with the byte mask to remove the high byte
return returnBytes; //return byte array
}


byte intToBytesHIGH(int convertInt)
{
byte returnBytes; // array that holds the two bytes to return
byte mask = byte(255); //mask for the low byte (255/0xff)
returnBytes = byte((convertInt >> 8) & mask);//high byte - shift the byte to the right 8 bits. perform an '&' operation with the byte mask to remove any additional data
return returnBytes; //return byte array
}

kgranat
04-27-2015, 11:04 AM
Were you able to get the software working with the stock armlink example firmware?

Can you give us a broader overview of what you're trying to accomplish? Will the computer be taking inputs from your fingertips, or are you doing it at the microcontroller level?

If you're trying to control the arm from a computer, I would reccomend using the stock InterbotixArmLinkSerial firmware, then send commands to the arm from your computer - you shouldn't need to modify the firmware (unless you need custom features). All of the custom code will be on your computer app.

The armlink library facilitates the armlink serial protocol, but it doesn't actually run any arm IK. So you still need the IK code that is in the other files

https://github.com/Interbotix/ArmLinkLibrary/tree/master/ArmLink/examples/InterbotixArmLinkSerial


This demo also has some code for standalone use of the arms IK engine.
http://learn.trossenrobotics.com/20-interbotix/robot-arms/143-arm-link-sequence-playback.html

EpicZa
04-27-2015, 11:42 AM
Were you able to get the software working with the stock armlink example firmware?

I take it you mean this one: http://learn.trossenrobotics.com/20-interbotix/robot-arms/143-arm-link-sequence-playback.html

Yes, it all works correctly.


Can you give us a broader overview of what you're trying to accomplish? Will the computer be taking inputs from your fingertips, or are you doing it at the microcontroller level?

Have you heard of the Leap Motion (https://www.leapmotion.com/) Essentially I already have a C++ program that takes the coordinates of the users finer tips, (x,y,z), so to begin with I would like to hard code (x,y,z) coords just to get it working, then move on to take the coords from the Leap Motion directly and get the arm to follow the users finger tip.


If you're trying to control the arm from a computer, I would reccomend using the stock InterbotixArmLinkSerial firmware, then send commands to the arm from your computer - you shouldn't need to modify the firmware (unless you need custom features). All of the custom code will be on your computer app.

The armlink library facilitates the armlink serial protocol, but it doesn't actually run any arm IK. So you still need the IK code that is in the other files

https://github.com/Interbotix/ArmLinkLibrary/tree/master/ArmLink/examples/InterbotixArmLinkSerial

Yes, this verifies and uploads, but from what I gather you need a controller to send movements to the arm?

For example:
armlink.buttons


//================================================== =================================================
// Check EXT packet to determine action
//================================================== =================================================
void ExtArmState(){
if(armlink.ext < 0x10){
// no action
g_fArmActive = true;
}

else if(armlink.ext == 0x11){ //96
EmergencyStop();
IDPacket();
}....


I'm not really sure how I control the arm with interbotixArmLinkSerial. I should point out I only have a pincher arm and a arbotix-m.


This demo also has some code for standalone use of the arms IK engine.
http://learn.trossenrobotics.com/20-interbotix/robot-arms/143-arm-link-sequence-playback.html

Ok yes now I understand that I need more than what I posted above, can't just send a packet.

The sequence firmware looks promising, I'll see if I can get it to move through its maximum working area, then maybe I'll be able to figure a way to send it the coordinates automatically.

Thanks for the info.

EDIT: Ok for some reason the sequence example is not working, I turned the button value to false and uncommented pincher, it verifies and uploads, arm moves to resting position, then outputs rubbish:

Žàüpþüÿðüpÿàü€ààpŽüüðüpþàþ üàüàþüðüàüþüüþüà€üàŽàüüp à€àüàpøàüàpàü€üþàþüÿàüüà ðàþþàþàüüüàüðüŽüþüüþàüpŽà üüpðà€àüààðàüàààü€üþàþüÿ €ààüþüàüüàüðàþüàüüüàüðüŽ üþüüàpþüüàüðüŽüþüüàüüàüðü üàüðàþüà€|ðàþüÿàüü~àüààþà üàüŽàŽüà€

And doesn't play the sequence.

EDIT2: Ok turns out I had to remove all the button/pin defines, and now runs the sequence.

kgranat
04-28-2015, 10:05 AM
The ArmLinkSerial firmware is the firmware you loaded onto your pincher arm so that the armlink program could send commands to the arm. Basically, if you ran through this getting started guide to control the arm from our program, you've loaded the firmware onto your arm
http://learn.trossenrobotics.com/20-interbotix/robot-arms/137-interbotix-arm-link-software.html

I've worked with the leap motion before, and did some basic demos of moving one of our arms with it. I used the ArmLinkSerial firmware (unmodifired) on the arm, the used the computer as the controller. I opened up a serial port in Java, then sent command packets over to the arm to the XYZ coordinates I wanted the arm to go to based on the leapmotion input.

EpicZa
04-30-2015, 08:13 AM
The ArmLinkSerial firmware is the firmware you loaded onto your pincher arm so that the armlink program could send commands to the arm. Basically, if you ran through this getting started guide to control the arm from our program, you've loaded the firmware onto your arm
http://learn.trossenrobotics.com/20-interbotix/robot-arms/137-interbotix-arm-link-software.html

I've worked with the leap motion before, and did some basic demos of moving one of our arms with it. I used the ArmLinkSerial firmware (unmodifired) on the arm, the used the computer as the controller. I opened up a serial port in Java, then sent command packets over to the arm to the XYZ coordinates I wanted the arm to go to based on the leapmotion input.

Yea it just took me a while getting my head around it, getting there now, creating code in c++ now to send the packets over serial.

thanks for the Help!

kgranat
04-30-2015, 09:30 AM
Glad we could help. Make sure to show us some videos when you get everything up and running!

EpicZa
04-30-2015, 11:31 AM
Glad we could help. Make sure to show us some videos when you get everything up and running!

Spoke too soon! haha, I modified the ArmLinkSerial to take strings of text and spilt them up into coordinates, so I'd send 010001000100 for (100,100,100), and that would move the robot.

So I thought I'd move onto trying to send it command packets with the original InterbotixArmLinkSerial uploaded to the board, doesn't seem to work. Connects to the board, but just outputs a smilie face symbol, so its laughing at me!

But yes if I get it working I'll upload a few videos.

kgranat
04-30-2015, 01:27 PM
Do you have your baud set to 38400?

What are you using to send the command packets?

EpicZa
05-01-2015, 05:51 AM
I'm using this to send across the serial http://playground.arduino.cc/Interfacing/CPPWindows#VisualStudio2008

Yes it wasn't working initially at all and I changed the baud, which allowed me to send the strings across. If I code up a program and upload it to the arbotix and create my own c++ code it works, but I can't seem to get it to work with the
interbotixArmLinkSerial.

EDIT: Took out code.

kgranat
05-07-2015, 11:39 AM
Correct me if I'm wrong, but your code looks like you're sending strings instead of bytes. So you're actually sending the ASCII characters "0xff' instead of the value 0xff

So if you take a look at the ArmLinkSoftware function for putting a packet together
https://github.com/Interbotix/ArmLinkSoftware/blob/master/ArmLink/arbotix.pde#L471-L524
You'll notice that we're never dealing with strings of appending the '0x' to any characters. You might want to do that for your console output, but you want to send the raw values over the serial port.


Also, where are you setting your baud rate?


Are you working in visual studio or in another environment?

EpicZa
05-07-2015, 11:53 AM
Correct me if I'm wrong, but your code looks like you're sending strings instead of bytes. So you're actually sending the ASCII characters "0xff' instead of the value 0xff

So if you take a look at the ArmLinkSoftware function for putting a packet together
https://github.com/Interbotix/ArmLinkSoftware/blob/master/ArmLink/arbotix.pde#L471-L524
You'll notice that we're never dealing with strings of appending the '0x' to any characters. You might want to do that for your console output, but you want to send the raw values over the serial port.


Also, where are you setting your baud rate?


Are you working in visual studio or in another environment?

Yea I know that's why I deleted the post, I realised after I went back and checked, the output to console (you one you've linked) is cast to hex. So yea I was sending string hex, I'm learning along the way!

Sorry, I seem to always post and then realise what I've done wrong!

http://playground.arduino.cc/Interfacing/CPPWindows#VisualStudio2008

this line
dcbSerialParams.BaudRate=CBR_9600;

I changed it.

Yes visual studio.

kgranat
05-08-2015, 12:08 PM
No need to be sorry, this is how we all learn :)

I'm also always looking for better ways to explain to people sending raw values vs sending ascii. Since the Arduino IDE's console doesn't support sending raw values, it can be tricky to get your head around at first.

Any luck communicating with the arm?

EpicZa
05-11-2015, 04:50 AM
No need to be sorry, this is how we all learn :)

I'm also always looking for better ways to explain to people sending raw values vs sending ascii. Since the Arduino IDE's console doesn't support sending raw values, it can be tricky to get your head around at first.

Any luck communicating with the arm?

Unfortunately not, 'in theory' this should work, and its not throwing any errors to the console, on occasion the arduino will send back "ffffffffff". Seems to do it some times and then just not bother.

Making progress but still not quite there!

If I get it working I'll definitely send you it to use as an example if you want.

kgranat
05-15-2015, 12:58 PM
Are you able to export your program to some kind of executable? That way I can analyze the actual packets coming out of your program.

Also, can you e-mail me your order number for the arm?
kyle@trossenrobotics.com

EpicZa
05-15-2015, 02:27 PM
Are you able to export your program to some kind of executable? That way I can analyze the actual packets coming out of your program.

Also, can you e-mail me your order number for the arm?
kyle@trossenrobotics.com


I fixed it thanks, its working.. for now..

Silly mistake was sending the header as "0xFF" as a string instead of just 0xFF so it was reading in the header as 4 chars 0, x, F, and F. Didn't even think to look there all this time!

EpicZa
05-25-2015, 04:58 AM
When the robot moves to a point, is it making reference to the tip of the gripper, or the centre of the gripper, or the 5th servo? Making progress just trying to set some limits, rather jerky and seems like its going to tear itself apart!

kgranat
05-26-2015, 11:00 AM
The IK engine assumes that the middle of the gripper is the end point.

What are you setting your delta value at?

Keep in mind that the delta value is not a direct velocity value, but a time value - so 2 movements with the same delta value will have 2 different speeds if they have 2 different distances to travel. So large movements will have a higher velocity than small movements. You may need to vary your delta based on how far the movement is.

EpicZa
05-26-2015, 12:27 PM
The IK engine assumes that the middle of the gripper is the end point.

What are you setting your delta value at?

Keep in mind that the delta value is not a direct velocity value, but a time value - so 2 movements with the same delta value will have 2 different speeds if they have 2 different distances to travel. So large movements will have a higher velocity than small movements. You may need to vary your delta based on how far the movement is.

Ok thanks.

Well that changes everything! Hmm, can't believe I've over looked that! I generally have it at 32, sometimes maybe a bit lower, I'll code in some variations depending on the distance moved.

It may also be because the board and robot can't keep up with the data I'm sending it. Leap is around 300fps, I've tried averaging 5 frames from the Leap, and just taking every 5th, doesn't really smooth it out.

Maybe some form of prediction, I've found if I'm moving my hand in one plane slowly the robot seems to jutter a few coordinates behind, rather than staying close to the finger. (I've already changed the fWait to false in MoveArmTo on the board itself, so it doesn't complete every coordinate I send it).

I also think I'm expecting to smooth movements from the servos, the 1mm steps are maybe to large, for what I'm trying to do.

Definitely haven't given up though, Im close!

Thanks again.