Difference between revisions of "PD control api"

Line 7: Line 7:
 
<br>
 
<br>
  
−
-
+
- Make 2 files under grasp directory.
 
<syntaxhighlight lang="cpp">
 
<syntaxhighlight lang="cpp">
−
 
+
RSP.cpp
 +
RSP.h
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
-
+
- RSP.h
 
<syntaxhighlight lang="cpp">
 
<syntaxhighlight lang="cpp">
 +
#ifndef _ROCKSCISSORSPAPER_H
 +
#define _ROCKSCISSORSPAPER_H
  
 +
void MotionRock();
 +
void MotionScissors();
 +
void MotionPaper();
 +
 +
#endif
 
</syntaxhighlight>
 
</syntaxhighlight>
  
−
-
+
- RSP.cpp
 
<syntaxhighlight lang="cpp">
 
<syntaxhighlight lang="cpp">
 +
#include "rDeviceAllegroHandCANDef.h"
 +
#include <BHand/BHand.h>
 +
 +
// ROCK-SCISSORS-PAPER(RIGHT HAND)
 +
static double rock[] = {
 +
-0.1194, 1.2068, 1.0, 1.4042,
 +
-0.0093, 1.2481, 1.4073, 0.8163,
 +
0.1116, 1.2712, 1.3881, 1.0122,
 +
0.6017, 0.2976, 0.9034, 0.7929};
 +
static double paper[] = {
 +
-0.1220, 0.4, 0.6, -0.0769,
 +
0.0312, 0.4, 0.6, -0.0,
 +
0.1767, 0.4, 0.6, -0.0528,
 +
0.5284, 0.3693, 0.8977, 0.4863};
 +
static double scissors[] = {
 +
0.0885, 0.4, 0.6, -0.0704,
 +
0.0312, 0.4, 0.6, -0.0,
 +
0.1019, 1.2375, 1.1346,
 +
1.0244, 1.0, 0.6331, 1.3509, 1.0};
 +
 +
 +
extern BHand* pBHand;
 +
extern double q_des[MAX_DOF];
 +
 +
static void SetGainsRSP()
 +
{
 +
// This function should be called after the function SetMotionType() is called.
 +
// Once SetMotionType() function is called, all gains are reset using the default values.
 +
if (!pBHand) return;
 +
double kp[] = {
 +
500, 800, 900, 500,
 +
500, 800, 900, 500,
 +
500, 800, 900, 500,
 +
1000, 700, 600, 600
 +
};
 +
double kd[] = {
 +
25, 50, 55, 40,
 +
25, 50, 55, 40,
 +
25, 50, 55, 40,
 +
50, 50, 50, 40
 +
};
 +
pBHand->SetGainsEx(kp, kd);
 +
}
 +
 +
void MotionRock()
 +
{
 +
for (int i=0; i<16; i++)
 +
q_des[i] = rock[i];
 +
if (pBHand) pBHand->SetMotionType(eMotionType_JOINT_PD);
 +
SetGainsRSP();
 +
 +
}
 +
 +
void MotionScissors()
 +
{
 +
for (int i=0; i<16; i++)
 +
q_des[i] = scissors[i];
 +
if (pBHand) pBHand->SetMotionType(eMotionType_JOINT_PD);
 +
SetGainsRSP();
 +
}
  
 +
void MotionPaper()
 +
{
 +
for (int i=0; i<16; i++)
 +
q_des[i] = paper[i];
 +
if (pBHand) pBHand->SetMotionType(eMotionType_JOINT_PD);
 +
SetGainsRSP();
 +
}
 
</syntaxhighlight>
 
</syntaxhighlight>
  

Revision as of 17:16, 3 July 2019

Making Rock Paper Scissors by Allegro Hand

This instruction shows you how to use PD control api by making Rock Paper Scissors with Allegro Hand.

Please download BHand_PDcontrol_tutorial.zip file and follow below steps. File:BHand PDcontrol tutorial.zip
Also, you need to install BHand library. "libBHand": Grasping_Library_for_Linux

- Make 2 files under grasp directory. 

RSP.cpp
RSP.h

- RSP.h 

#ifndef _ROCKSCISSORSPAPER_H
#define _ROCKSCISSORSPAPER_H
 
void MotionRock();
void MotionScissors();
void MotionPaper();
 
#endif

- RSP.cpp 

#include "rDeviceAllegroHandCANDef.h"
#include <BHand/BHand.h>
 
// ROCK-SCISSORS-PAPER(RIGHT HAND)
static double rock[] = {
	-0.1194, 1.2068, 1.0, 1.4042,
	-0.0093, 1.2481, 1.4073, 0.8163,
	0.1116, 1.2712, 1.3881, 1.0122,
	0.6017, 0.2976, 0.9034, 0.7929};
static double paper[] = {
	-0.1220, 0.4, 0.6, -0.0769,
	0.0312, 0.4, 0.6, -0.0,
	0.1767, 0.4, 0.6, -0.0528,
	0.5284, 0.3693, 0.8977, 0.4863};
static double scissors[] = {
	0.0885, 0.4, 0.6, -0.0704,
	0.0312, 0.4, 0.6, -0.0,
	0.1019, 1.2375, 1.1346,
	1.0244, 1.0, 0.6331, 1.3509, 1.0};
 
 
extern BHand* pBHand;
extern double q_des[MAX_DOF];
 
static void SetGainsRSP()
{
	// This function should be called after the function SetMotionType() is called.
	// Once SetMotionType() function is called, all gains are reset using the default values.
	if (!pBHand) return;
	double kp[] = {
		500, 800, 900, 500,
		500, 800, 900, 500,
		500, 800, 900, 500,
		1000, 700, 600, 600
	};
	double kd[] = {
		25, 50, 55, 40,
		25, 50, 55, 40,
		25, 50, 55, 40,
		50, 50, 50, 40
	};
	pBHand->SetGainsEx(kp, kd);
}
 
void MotionRock()
{
	for (int i=0; i<16; i++)
		q_des[i] = rock[i];
	if (pBHand) pBHand->SetMotionType(eMotionType_JOINT_PD);
	SetGainsRSP();
 
}
 
void MotionScissors()
{
	for (int i=0; i<16; i++)
		q_des[i] = scissors[i];
	if (pBHand) pBHand->SetMotionType(eMotionType_JOINT_PD);
	SetGainsRSP();
}
 
void MotionPaper()
{
	for (int i=0; i<16; i++)
		q_des[i] = paper[i];
	if (pBHand) pBHand->SetMotionType(eMotionType_JOINT_PD);
	SetGainsRSP();
}

- 

 

- 

 

- 

 

- 

 




Whos here now:   Members 0   Guests 0   Bots & Crawlers 1