Skip to content
GitLab
Next
About GitLab
GitLab: the DevOps platform
Explore GitLab
Install GitLab
How GitLab compares
Get started
GitLab docs
GitLab Learn
Pricing
Talk to an expert
/
Help
What's new
2
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Projects
Groups
Snippets
Sign up now
Login
Sign in / Register
Toggle navigation
Menu
Open sidebar
Haply
2diy
learn
Commits
1d6c520b
Commit
1d6c520b
authored
Jan 25, 2021
by
Oliver Anthony
Browse files
V04
parents
Changes
27
Hide whitespace changes
Inline
Side-by-side
sketch_1_Device_Setup/Pantograph.java
0 → 100644
View file @
1d6c520b
/**
**********************************************************************************************************************
* @file Pantograph.java
* @author Steve Ding, Colin Gallacher
* @version V3.0.0
* @date 15-January-2021
* @brief Mechanism extension example
**********************************************************************************************************************
* @attention
*
*
**********************************************************************************************************************
*/
import
static
java
.
lang
.
Math
.*;
public
class
Pantograph
extends
Mechanisms
{
private
float
l
,
L
,
d
;
private
float
th1
,
th2
;
private
float
tau1
,
tau2
;
private
float
f_x
,
f_y
;
private
float
x_E
,
y_E
;
private
float
pi
=
3.14159265359f
;
private
float
JT11
,
JT12
,
JT21
,
JT22
;
private
float
gain
=
1.0f
;
public
Pantograph
(){
this
.
l
=
0.07f
;
this
.
L
=
0.09f
;
this
.
d
=
0.0f
;
}
public
void
torqueCalculation
(
float
[]
force
){
f_x
=
force
[
0
];
f_y
=
force
[
1
];
tau1
=
JT11
*
f_x
+
JT12
*
f_y
;
tau2
=
JT21
*
f_x
+
JT22
*
f_y
;
tau1
=
tau1
*
gain
;
tau2
=
tau2
*
gain
;
}
public
void
forwardKinematics
(
float
[]
angles
){
float
l1
=
l
;
float
l2
=
l
;
float
L1
=
L
;
float
L2
=
L
;
th1
=
pi
/
180
*
angles
[
0
];
th2
=
pi
/
180
*
angles
[
1
];
// Forward Kinematics
float
c1
=
(
float
)
cos
(
th1
);
float
c2
=
(
float
)
cos
(
th2
);
float
s1
=
(
float
)
sin
(
th1
);
float
s2
=
(
float
)
sin
(
th2
);
float
xA
=
l1
*
c1
;
float
yA
=
l1
*
s1
;
float
xB
=
d
+
l2
*
c2
;
float
yB
=
l2
*
s2
;
float
hx
=
xB
-
xA
;
float
hy
=
yB
-
yA
;
float
hh
=
(
float
)
pow
(
hx
,
2
)
+
(
float
)
pow
(
hy
,
2
);
float
hm
=
(
float
)
sqrt
(
hh
);
float
cB
=
-
((
float
)
pow
(
L2
,
2
)
-
(
float
)
pow
(
L1
,
2
)
-
hh
)
/
(
2
*
L1
*
hm
);
float
h1x
=
L1
*
cB
*
hx
/
hm
;
float
h1y
=
L1
*
cB
*
hy
/
hm
;
float
h1h1
=
(
float
)
pow
(
h1x
,
2
)
+
(
float
)
pow
(
h1y
,
2
);
float
h1m
=
(
float
)
sqrt
(
h1h1
);
float
sB
=
(
float
)
sqrt
(
1
-
pow
(
cB
,
2
));
float
lx
=
-
L1
*
sB
*
h1y
/
h1m
;
float
ly
=
L1
*
sB
*
h1x
/
h1m
;
float
x_P
=
xA
+
h1x
+
lx
;
float
y_P
=
yA
+
h1y
+
ly
;
float
phi1
=
(
float
)
acos
((
x_P
-
l1
*
c1
)/
L1
);
float
phi2
=
(
float
)
acos
((
x_P
-
d
-
l2
*
c2
)/
L2
);
float
c11
=
(
float
)
cos
(
phi1
);
float
s11
=(
float
)
sin
(
phi1
);
float
c22
=
(
float
)
cos
(
phi2
);
float
s22
=
(
float
)
sin
(
phi2
);
float
dn
=
L1
*(
c11
*
s22
-
c22
*
s11
);
float
eta
=
(-
L1
*
c11
*
s22
+
L1
*
c22
*
s11
-
c1
*
l1
*
s22
+
c22
*
l1
*
s1
)
/
dn
;
float
nu
=
l2
*
(
c2
*
s22
-
c22
*
s2
)/
dn
;
JT11
=
-
L1
*
eta
*
s11
-
L1
*
s11
-
l1
*
s1
;
JT12
=
L1
*
c11
*
eta
+
L1
*
c11
+
c1
*
l1
;
JT21
=
-
L1
*
s11
*
nu
;
JT22
=
L1
*
c11
*
nu
;
x_E
=
x_P
;
y_E
=
y_P
;
}
public
void
forceCalculation
(){
}
public
void
positionControl
(){
}
public
void
inverseKinematics
(){
}
public
void
set_mechanism_parameters
(
float
[]
parameters
){
this
.
l
=
parameters
[
0
];
this
.
L
=
parameters
[
1
];
this
.
d
=
parameters
[
2
];
}
public
void
set_sensor_data
(
float
[]
data
){
}
public
float
[]
get_coordinate
(){
float
temp
[]
=
{
x_E
,
y_E
};
return
temp
;
}
public
float
[]
get_torque
(){
float
temp
[]
=
{
tau1
,
tau2
};
return
temp
;
}
public
float
[]
get_angle
(){
float
temp
[]
=
{
th1
,
th2
};
return
temp
;
}
}
sketch_1_Device_Setup/code/hAPI_V04.jar
0 → 100644
View file @
1d6c520b
File added
sketch_1_Device_Setup/sketch_1_Device_Setup.pde
0 → 100644
View file @
1d6c520b
/**
**********************************************************************************************************************
* @file sketch_1_Device_Setup.pde
* @author Steve Ding, Colin Gallacher
* @version V1.0.0
* @date 08-January-2021
* @brief hAPI device setup and move end effector in 2D environment
**********************************************************************************************************************
* @attention
*
*
**********************************************************************************************************************
*/
/* library imports *****************************************************************************************************/
import
processing
.
serial
.
*
;
import
static
java
.
util
.
concurrent
.
TimeUnit
.
*
;
import
java
.
util
.
concurrent
.
*
;
/* end library imports *************************************************************************************************/
/* scheduler definition ************************************************************************************************/
private
final
ScheduledExecutorService
scheduler
=
Executors
.
newScheduledThreadPool
(
1
);
/* end scheduler definition ********************************************************************************************/
/* device block definitions ********************************************************************************************/
Board
haplyBoard
;
Device
widgetOne
;
Mechanisms
pantograph
;
byte
widgetOneID
=
5
;
int
CW
=
0
;
int
CCW
=
1
;
boolean
rendering_force
=
false
;
/* end device block definition *****************************************************************************************/
/* framerate definition ************************************************************************************************/
long
baseFrameRate
=
120
;
/* end framerate definition ********************************************************************************************/
/* elements definition *************************************************************************************************/
/* Screen and world setup parameters */
float
pixelsPerMeter
=
4000.0
;
/* generic data for a 2DOF device */
/* joint space */
PVector
angles
=
new
PVector
(
0
,
0
);
PVector
torques
=
new
PVector
(
0
,
0
);
/* task space */
PVector
posEE
=
new
PVector
(
0
,
0
);
PVector
fEE
=
new
PVector
(
0
,
0
);
/* World boundaries reference */
final
int
worldPixelWidth
=
1000
;
final
int
worldPixelHeight
=
650
;
/* Initialization of virtual tool */
PShape
eeAvatar
;
/* end elements definition *********************************************************************************************/
/* setup section *******************************************************************************************************/
void
setup
(){
/* put setup code here, run once: */
/* screen size definition */
size
(
1000
,
650
);
/* device setup */
/**
* The board declaration needs to be changed depending on which USB serial port the Haply board is connected.
* In the base example, a connection is setup to the first detected serial device, this parameter can be changed
* to explicitly state the serial port will look like the following for different OS:
*
* windows: haplyBoard = new Board(this, "COM10", 0);
* linux: haplyBoard = new Board(this, "/dev/ttyUSB0", 0);
* mac: haplyBoard = new Board(this, "/dev/cu.usbmodem1411", 0);
*/
haplyBoard
=
new
Board
(
this
,
"COM4"
,
0
);
widgetOne
=
new
Device
(
widgetOneID
,
haplyBoard
);
pantograph
=
new
Pantograph
();
widgetOne
.
set_mechanism
(
pantograph
);
widgetOne
.
add_actuator
(
1
,
CCW
,
2
);
widgetOne
.
add_actuator
(
2
,
CW
,
1
);
widgetOne
.
add_encoder
(
1
,
CCW
,
241
,
10752
,
2
);
widgetOne
.
add_encoder
(
2
,
CW
,
-
61
,
10752
,
1
);
widgetOne
.
device_set_parameters
();
eeAvatar
=
createShape
(
ELLIPSE
,
0
,
0
,
50
,
50
);
eeAvatar
.
setFill
(
color
(
0
));
/* setup framerate speed */
frameRate
(
baseFrameRate
);
/* setup simulation thread to run at 1kHz */
SimulationThread
st
=
new
SimulationThread
();
scheduler
.
scheduleAtFixedRate
(
st
,
1
,
1
,
MILLISECONDS
);
}
/* end setup section ***************************************************************************************************/
/* draw section ********************************************************************************************************/
void
draw
(){
/* put graphical code here, runs repeatedly at defined framerate in setup, else default at 60fps: */
background
(
255
);
update_end_effector
(
posEE
.
x
,
posEE
.
y
);
}
/* end draw section ****************************************************************************************************/
/* simulation section **************************************************************************************************/
class
SimulationThread
implements
Runnable
{
public
void
run
(){
/* put haptic simulation code here, runs repeatedly at 1kHz as defined in setup */
rendering_force
=
true
;
if
(
haplyBoard
.
data_available
()){
/* GET END-EFFECTOR STATE (TASK SPACE) */
widgetOne
.
device_read_data
();
angles
.
set
(
widgetOne
.
get_device_angles
());
posEE
.
set
(
widgetOne
.
get_device_position
(
angles
.
array
()));
posEE
.
set
(
device_to_graphics
(
posEE
));
}
torques
.
set
(
widgetOne
.
set_device_torques
(
fEE
.
array
()));
widgetOne
.
device_write_torques
();
rendering_force
=
false
;
}
}
/* end simulation section **********************************************************************************************/
/* helper functions section, place helper functions here ***************************************************************/
void
update_end_effector
(
float
xE
,
float
yE
){
background
(
255
);
xE
=
(
pixelsPerMeter
*
xE
)
+
worldPixelWidth
/
2
;
yE
=
(
pixelsPerMeter
*
yE
);
translate
(
xE
,
yE
);
shape
(
eeAvatar
);
}
PVector
device_to_graphics
(
PVector
deviceFrame
){
return
deviceFrame
.
set
(
-
deviceFrame
.
x
,
deviceFrame
.
y
);
}
/* end helper functions section ****************************************************************************************/
sketch_2_Hello_Wall/Pantograph.java
0 → 100644
View file @
1d6c520b
/**
**********************************************************************************************************************
* @file Pantograph.java
* @author Steve Ding, Colin Gallacher
* @version V3.0.0
* @date 15-January-2021
* @brief Mechanism extension example
**********************************************************************************************************************
* @attention
*
*
**********************************************************************************************************************
*/
import
static
java
.
lang
.
Math
.*;
public
class
Pantograph
extends
Mechanisms
{
private
float
l
,
L
,
d
;
private
float
th1
,
th2
;
private
float
tau1
,
tau2
;
private
float
f_x
,
f_y
;
private
float
x_E
,
y_E
;
private
float
pi
=
3.14159265359f
;
private
float
JT11
,
JT12
,
JT21
,
JT22
;
private
float
gain
=
1.0f
;
public
Pantograph
(){
this
.
l
=
0.07f
;
this
.
L
=
0.09f
;
this
.
d
=
0.0f
;
}
public
void
torqueCalculation
(
float
[]
force
){
f_x
=
force
[
0
];
f_y
=
force
[
1
];
tau1
=
JT11
*
f_x
+
JT12
*
f_y
;
tau2
=
JT21
*
f_x
+
JT22
*
f_y
;
tau1
=
tau1
*
gain
;
tau2
=
tau2
*
gain
;
}
public
void
forwardKinematics
(
float
[]
angles
){
float
l1
=
l
;
float
l2
=
l
;
float
L1
=
L
;
float
L2
=
L
;
th1
=
pi
/
180
*
angles
[
0
];
th2
=
pi
/
180
*
angles
[
1
];
// Forward Kinematics
float
c1
=
(
float
)
cos
(
th1
);
float
c2
=
(
float
)
cos
(
th2
);
float
s1
=
(
float
)
sin
(
th1
);
float
s2
=
(
float
)
sin
(
th2
);
float
xA
=
l1
*
c1
;
float
yA
=
l1
*
s1
;
float
xB
=
d
+
l2
*
c2
;
float
yB
=
l2
*
s2
;
float
hx
=
xB
-
xA
;
float
hy
=
yB
-
yA
;
float
hh
=
(
float
)
pow
(
hx
,
2
)
+
(
float
)
pow
(
hy
,
2
);
float
hm
=
(
float
)
sqrt
(
hh
);
float
cB
=
-
((
float
)
pow
(
L2
,
2
)
-
(
float
)
pow
(
L1
,
2
)
-
hh
)
/
(
2
*
L1
*
hm
);
float
h1x
=
L1
*
cB
*
hx
/
hm
;
float
h1y
=
L1
*
cB
*
hy
/
hm
;
float
h1h1
=
(
float
)
pow
(
h1x
,
2
)
+
(
float
)
pow
(
h1y
,
2
);
float
h1m
=
(
float
)
sqrt
(
h1h1
);
float
sB
=
(
float
)
sqrt
(
1
-
pow
(
cB
,
2
));
float
lx
=
-
L1
*
sB
*
h1y
/
h1m
;
float
ly
=
L1
*
sB
*
h1x
/
h1m
;
float
x_P
=
xA
+
h1x
+
lx
;
float
y_P
=
yA
+
h1y
+
ly
;
float
phi1
=
(
float
)
acos
((
x_P
-
l1
*
c1
)/
L1
);
float
phi2
=
(
float
)
acos
((
x_P
-
d
-
l2
*
c2
)/
L2
);
float
c11
=
(
float
)
cos
(
phi1
);
float
s11
=(
float
)
sin
(
phi1
);
float
c22
=
(
float
)
cos
(
phi2
);
float
s22
=
(
float
)
sin
(
phi2
);
float
dn
=
L1
*(
c11
*
s22
-
c22
*
s11
);
float
eta
=
(-
L1
*
c11
*
s22
+
L1
*
c22
*
s11
-
c1
*
l1
*
s22
+
c22
*
l1
*
s1
)
/
dn
;
float
nu
=
l2
*
(
c2
*
s22
-
c22
*
s2
)/
dn
;
JT11
=
-
L1
*
eta
*
s11
-
L1
*
s11
-
l1
*
s1
;
JT12
=
L1
*
c11
*
eta
+
L1
*
c11
+
c1
*
l1
;
JT21
=
-
L1
*
s11
*
nu
;
JT22
=
L1
*
c11
*
nu
;
x_E
=
x_P
;
y_E
=
y_P
;
}
public
void
forceCalculation
(){
}
public
void
positionControl
(){
}
public
void
inverseKinematics
(){
}
public
void
set_mechanism_parameters
(
float
[]
parameters
){
this
.
l
=
parameters
[
0
];
this
.
L
=
parameters
[
1
];
this
.
d
=
parameters
[
2
];
}
public
void
set_sensor_data
(
float
[]
data
){
}
public
float
[]
get_coordinate
(){
float
temp
[]
=
{
x_E
,
y_E
};
return
temp
;
}
public
float
[]
get_torque
(){
float
temp
[]
=
{
tau1
,
tau2
};
return
temp
;
}
public
float
[]
get_angle
(){
float
temp
[]
=
{
th1
,
th2
};
return
temp
;
}
}
sketch_2_Hello_Wall/code/hAPI_V04.jar
0 → 100644
View file @
1d6c520b
File added
sketch_2_Hello_Wall/sketch_2_Hello_Wall.pde
0 → 100644
View file @
1d6c520b
/**
**********************************************************************************************************************
* @file sketch_2_Hello_Wall.pde
* @author Steve Ding, Colin Gallacher
* @version V3.0.0
* @date 08-January-2021
* @brief Wall haptic example with programmed physics for a haptic wall
**********************************************************************************************************************
* @attention
*
*
**********************************************************************************************************************
*/
/* library imports *****************************************************************************************************/
import
processing
.
serial
.
*
;
import
static
java
.
util
.
concurrent
.
TimeUnit
.
*
;
import
java
.
util
.
concurrent
.
*
;
/* end library imports *************************************************************************************************/
/* scheduler definition ************************************************************************************************/
private
final
ScheduledExecutorService
scheduler
=
Executors
.
newScheduledThreadPool
(
1
);
/* end scheduler definition ********************************************************************************************/
/* device block definitions ********************************************************************************************/
Board
haplyBoard
;
Device
widgetOne
;
Mechanisms
pantograph
;
byte
widgetOneID
=
5
;
int
CW
=
0
;
int
CCW
=
1
;
boolean
renderingForce
=
false
;
/* end device block definition *****************************************************************************************/
/* framerate definition ************************************************************************************************/
long
baseFrameRate
=
120
;
/* end framerate definition ********************************************************************************************/
/* elements definition *************************************************************************************************/
/* Screen and world setup parameters */
float
pixelsPerMeter
=
4000.0
;
float
radsPerDegree
=
0.01745
;
/* pantagraph link parameters in meters */
float
l
=
0.07
;
float
L
=
0.09
;
/* end effector radius in meters */
float
rEE
=
0.006
;
/* virtual wall parameter */
float
kWall
=
450
;
PVector
fWall
=
new
PVector
(
0
,
0
);
PVector
penWall
=
new
PVector
(
0
,
0
);
PVector
posWall
=
new
PVector
(
0.01
,
0.10
);
/* generic data for a 2DOF device */
/* joint space */
PVector
angles
=
new
PVector
(
0
,
0
);
PVector
torques
=
new
PVector
(
0
,
0
);
/* task space */
PVector
posEE
=
new
PVector
(
0
,
0
);
PVector
fEE
=
new
PVector
(
0
,
0
);
/* device graphical position */
PVector
deviceOrigin
=
new
PVector
(
0
,
0
);
/* World boundaries reference */
final
int
worldPixelWidth
=
1000
;
final
int
worldPixelHeight
=
650
;
/* graphical elements */
PShape
pGraph
,
joint
,
endEffector
;
PShape
wall
;
/* end elements definition *********************************************************************************************/
/* setup section *******************************************************************************************************/
void
setup
(){
/* put setup code here, run once: */
/* screen size definition */
size
(
1000
,
650
);
/* device setup */
/**
* The board declaration needs to be changed depending on which USB serial port the Haply board is connected.
* In the base example, a connection is setup to the first detected serial device, this parameter can be changed
* to explicitly state the serial port will look like the following for different OS:
*
* windows: haplyBoard = new Board(this, "COM10", 0);
* linux: haplyBoard = new Board(this, "/dev/ttyUSB0", 0);
* mac: haplyBoard = new Board(this, "/dev/cu.usbmodem1411", 0);
*/
haplyBoard
=
new
Board
(
this
,
Serial
.
list
()[
0