Skip to content

Commit

Permalink
neckmk3_2joints experimental setup (#689)
Browse files Browse the repository at this point in the history 
Co-authored-by: Ugo Pattacini <[email protected]>
  • Loading branch information
@SanLordKevin @pattacini
SanLordKevin and pattacini authored Nov 20, 2024
1 parent d66b1d6 commit de271a5
Show file tree
Hide file tree
Showing 18 changed files with 544 additions and 2 deletions.
15 changes: 15 additions & 0 deletions experimentalSetups/neckmk3_2joints/README.md
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
2 Joints Neck Setup
===================

This setup is a prototype of the neck that will be mounted on `iCub` and `ergoCub`.
It consists of two motors and can be actuated using `yarprobotinterface` and `yarpmotorgui`.
One motor is controlled by an `amc` board and the other by an `amcbldc` board.
All the parameters to move the setup are in the configuration files.

### Picture of the setup

| |
| :---: |
| ![](assets/neck_setup.jpg) |
| **Figure 1** Setup view |

Binary file added experimentalSetups/neckmk3_2joints/assets/neck_setup.jpg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
47 changes: 47 additions & 0 deletions experimentalSetups/neckmk3_2joints/calibrators/neck-calib.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<device xmlns:xi="http://www.w3.org/2001/XInclude" name="neck-calibrator" type="parametricCalibratorEth">

<xi:include href="../general.xml" />

<group name="GENERAL">
<param name="joints"> 2 </param> <!-- the number of joints of the robot part -->
<param name="deviceName"> Neckmk3_Calibrator </param>
</group>

<group name="HOME">
<param name="positionHome"> 0 0 </param>
<param name="velocityHome"> 10 10 </param>
</group>

<group name="CALIBRATION">
<param name="calibrationType"> 12 12 </param>

<param name="calibration1"> 36439 48191 </param>
<param name="calibration2"> 0 0 </param>
<param name="calibration3"> 0 0 </param>
<param name="calibration4"> 0 0 </param>
<param name="calibration5"> 0 0 </param>
<param name="calibrationZero"> 0 0 </param>
<param name="calibrationDelta"> 0 0 </param>

<param name="startupPosition"> 0.0 0.0 </param>
<param name="startupVelocity"> 10.0 10.0 </param>
<param name="startupMaxPwm"> 16000 16000 </param>
<param name="startupPosThreshold"> 2 2 </param>
</group>

<param name="CALIB_ORDER"> (0) (1) </param>

<action phase="startup" level="10" type="calibrate">
<param name="target"> neck-mc_remapper </param>
</action>

<action phase="interrupt1" level="1" type="park">
<param name="target"> neck-mc_remapper </param>
</action>

<action phase="interrupt3" level="1" type="abort" />

</device>
2 changes: 2 additions & 0 deletions experimentalSetups/neckmk3_2joints/firmwareupdater.ini
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
[DRIVERS]
ETH "eth"
11 changes: 11 additions & 0 deletions experimentalSetups/neckmk3_2joints/general.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="neckmk3" build="1">

<group name="GENERAL">
<param name="skipCalibration"> false </param>
<param name="useRawEncoderData"> false </param>
<param name="verbose"> false </param>
</group>
</params>
40 changes: 40 additions & 0 deletions experimentalSetups/neckmk3_2joints/hardware/electronics/neck-eb2-j0_1-eln.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="new_neck" build="1">

<xi:include href="./pc104.xml" />

<group name="ETH_BOARD">

<group name="ETH_BOARD_PROPERTIES">
<param name="IpAddress"> 10.0.1.1 </param>
<param name="IpPort"> 12345 </param>
<param name="Type"> amc </param>
<param name="maxSizeRXpacket"> 768 </param>
<param name="maxSizeROP"> 384 </param>
</group>

<group name="ETH_BOARD_SETTINGS">
<param name="Name"> "neck-eb2-j0_2" </param>
<group name="RUNNINGMODE">
<param name="period"> 1000 </param>
<param name="maxTimeOfRXactivity"> 400 </param>
<param name="maxTimeOfDOactivity"> 300 </param>
<param name="maxTimeOfTXactivity"> 300 </param>
<param name="TXrateOfRegularROPs"> 2 </param>
</group>
</group>

<group name="ETH_BOARD_ACTIONS">
<group name="MONITOR_ITS_PRESENCE">
<param name="enabled"> true </param>
<param name="timeout"> 0.020 </param>
<param name="periodOfMissingReport"> 60.0 </param>
</group>
</group>

</group>

</params>

14 changes: 14 additions & 0 deletions experimentalSetups/neckmk3_2joints/hardware/electronics/pc104.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="new_neck" build="1">

<group name="PC104">
<param name="PC104IpAddress"> 10.0.1.104 </param>
<param name="PC104IpPort"> 12345 </param>
<param name="PC104TXrate"> 1 </param>
<param name="PC104RXrate"> 5 </param>
</group>

</params>

93 changes: 93 additions & 0 deletions experimentalSetups/neckmk3_2joints/hardware/mechanicals/neck-eb2-j0_1-mec.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,93 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="new_neck" build="1">
<group name="GENERAL">
<param name="MotioncontrolVersion"> 6 </param>
<param name="Joints"> 2 </param>
<param name="AxisMap"> 0 1 </param>
<param name="AxisName"> "neck_pitch" "neck_roll" </param>
<param name="AxisType"> "revolute" "revolute" </param>
<param name="Encoder"> 182.044 182.044 </param>
<param name="fullscalePWM"> 32000 32000 </param>
<param name="ampsToSensor"> 1000.0 1000.0 </param>
<param name="Gearbox_M2J"> +100 +100 </param>
<param name="Gearbox_E2J"> 1 1 </param>
<param name="useMotorSpeedFbk"> 0 0 </param>
<param name="MotorType"> "FAULHABER" "FAULHABER" </param>
<param name="Verbose"> 0 0 </param>
</group>

<group name="LIMITS">
<param name="hardwareJntPosMax"> +22 +20 </param>
<param name="hardwareJntPosMin"> -30 -20 </param>
<param name="rotorPosMin"> 0 0 </param>
<param name="rotorPosMax"> 0 0 </param>
</group>

<group name="AMCBLDC">
<param name="HasHallSensor"> 1 1 </param>
<param name="TemperatureSensorType"> NONE NONE </param>
<param name="HasRotorEncoder"> 0 0 </param>
<param name="HasRotorEncoderIndex"> 0 0 </param>
<param name="HasSpeedEncoder"> 0 0 </param>
<param name="RotorIndexOffset"> 0 0 </param>
<param name="MotorPoles"> 14 14 </param>
</group>

<group name="ADVFOC">
<param name="Verbose"> 0 0 </param>
<param name="AutoCalibration"> 0 0 </param>
<param name="HasHallSensor"> 1 1 </param>
<param name="TemperatureSensorType"> NONE NONE </param>
<param name="HasRotorEncoder"> 0 0 </param>
<param name="HasRotorEncoderIndex"> 0 0 </param>
<param name="HasSpeedEncoder"> 0 0 </param>
<param name="RotorIndexOffset"> 0 0 </param>
<param name="MotorPoles"> 14 14 </param>
</group>


<group name="COUPLINGS">

<param name ="matrixJ2M">
1.0 0.0 0.0 0.0
0.0 1.0 0.0 0.0
0.0 0.0 1.0 0.0
0.0 0.0 0.0 1.0
</param>

<param name ="matrixM2J">
1.0 0.0 0.0 0.0
0.0 1.0 0.0 0.0
0.0 0.0 1.0 0.0
0.0 0.0 0.0 1.0
</param>

<param name ="matrixE2J">
1.00 0.00 0.00 0.00 0.00 0.00
0.00 1.00 0.00 0.00 0.00 0.00
0.00 0.00 1.00 0.00 0.00 0.00
0.00 0.00 0.00 1.00 0.00 0.00
</param>

</group>

<group name="JOINTSET_CFG">
<param name= "numberofsets"> 2 </param>
<group name="JOINTSET_0">
<param name="listofjoints"> 0 </param>
<param name="constraint"> none </param>
<param name="param1"> 0 </param>
<param name="param2"> 0 </param>
</group>
<group name="JOINTSET_1">
<param name="listofjoints"> 1 </param>
<param name="constraint"> none </param>
<param name="param1"> 0 </param>
<param name="param2"> 0 </param>
</group>
</group>

</params>

102 changes: 102 additions & 0 deletions experimentalSetups/neckmk3_2joints/hardware/motorControl/neck-eb2-j0_1-mc.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,102 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE devices PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">


<device xmlns:xi="http://www.w3.org/2001/XInclude" name="neck-eb2-j0_1-mc" type="embObjMotionControl">

<xi:include href="../../general.xml" />
<xi:include href="../../hardware/electronics/neck-eb2-j0_1-eln.xml" />
<xi:include href="../../hardware/mechanicals/neck-eb2-j0_1-mec.xml" />
<xi:include href="./neck-eb2-j0_1-mc_service.xml" />

<!-- joint number 0 1 -->
<!-- joint name neck-pitch neck-roll -->
<group name="LIMITS">
<param name="jntPosMax"> +20 +18 </param> <!-- using mechanical limits -2 -->
<param name="jntPosMin"> -28 -18 </param>
<param name="jntVelMax"> 50 50 </param>
<param name="motorNominalCurrents"> 1000 1000 </param>
<param name="motorPeakCurrents"> 2000 2000 </param>
<param name="motorOverloadCurrents"> 2100 2100 </param>
<param name="motorPwmLimit"> 16000 16000 </param>
</group>

<group name="TIMEOUTS">
<param name="velocity"> 100 100 </param>
</group>

<group name="IMPEDANCE">
<param name="stiffness"> 0 0 </param>
<param name="damping"> 0 0 </param>
</group>

<group name="CONTROLS">
<param name="positionControl"> POS_PID_DEFAULT POS_PID_DEFAULT </param>
<param name="velocityControl"> POS_PID_DEFAULT POS_PID_DEFAULT </param>
<param name="mixedControl"> POS_PID_DEFAULT POS_PID_DEFAULT </param>
<param name="torqueControl"> none none </param>
<param name="currentPid"> 2FOC_CUR_CONTROL 2FOC_CUR_CONTROL </param>
<param name="speedPid"> 2FOC_VEL_CONTROL 2FOC_VEL_CONTROL </param>
</group>


<!-- default position PID: begin -->

<group name="POS_PID_DEFAULT">
<param name="controlLaw"> minjerk </param>
<param name="outputType"> current </param>
<param name="fbkControlUnits"> metric_units </param>
<param name="outputControlUnits"> machine_units </param>
<param name="kp"> +100 +100 </param>
<param name="kd"> +12 +12 </param>
<param name="ki"> +80 +80 </param>
<param name="maxOutput"> 500 500 </param>
<param name="maxInt"> 200 200 </param>
<param name="stictionUp"> 0 0 </param>
<param name="stictionDown"> 0 0 </param>
<param name="kff"> 0 0 </param>
</group>

<!-- default position PID: end -->


<!-- other default PIDs: begin -->

<group name="2FOC_CUR_CONTROL">
<param name="controlLaw"> low_lev_current </param>
<param name="fbkControlUnits"> machine_units </param>
<param name="outputControlUnits"> machine_units </param>
<param name="kp"> 0.03 0.03 </param>
<param name="kd"> 0 0 </param>
<param name="ki"> 82 82 </param>
<param name="shift"> 0 0 </param>
<param name="maxOutput"> 16000 16000 </param>
<param name="maxInt"> 32000 32000 </param>
<param name="kff"> 0 0 </param>
</group>

<group name="2FOC_VEL_CONTROL">
<param name="controlLaw"> low_lev_speed </param>
<param name="fbkControlUnits"> machine_units </param>
<param name="outputControlUnits"> machine_units </param>
<param name="kff"> 0 0 </param>
<param name="kp"> 12 12 </param>
<param name="kd"> 0 0 </param>
<param name="ki"> 16 16 </param>
<param name="shift"> 10 10 </param>
<param name="maxOutput"> 32000 32000 </param>
<param name="maxInt"> 32000 32000 </param>
</group>

<!-- other default PIDs: end -->


<!-- custom PIDs: begin -->

<!-- custom PIDs: end -->

<group name="OTHER_CONTROL_PARAMETERS">
<param name="deadZone"> 0.0495 0.0495 </param>
</group>

</device>
59 changes: 59 additions & 0 deletions experimentalSetups/neckmk3_2joints/hardware/motorControl/neck-eb2-j0_1-mc_service.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE params PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">

<params xmlns:xi="http://www.w3.org/2001/XInclude" robot="new_neck" build="1">

<group name="SERVICE">

<param name="type"> eomn_serv_MC_advfoc </param>

<group name="PROPERTIES">

<group name="ETHBOARD">
<param name="type"> amc </param>
</group>

<group name="CANBOARDS">
<param name="type"> amc2c amcbldc </param>
<group name="PROTOCOL">
<param name="major"> 2 2 </param>
<param name="minor"> 0 0 </param>
</group>
<group name="FIRMWARE">
<param name="major"> 3 2 </param>
<param name="minor"> 1 1 </param>
<param name="build"> 0 0 </param>
</group>
</group>

<group name="JOINTMAPPING">

<group name="ACTUATOR">
<param name="type"> advfoc advfoc </param>
<param name="onboard"> amc2c amcbldc </param>
<param name="port"> ICC1:3 CAN1:1 </param>
</group>

<group name="ENCODER1">
<param name="type"> aea3 aea3 </param>
<param name="port"> CONN:J5_X1 CONN:J5_X2 </param>
<param name="position"> atjoint atjoint </param>
<param name="resolution"> 16384 16384 </param>
<param name="tolerance"> 0.4 0.4 </param>
</group>

<group name="ENCODER2">
<param name="type"> none none </param>
<param name="port"> CAN1:1:0 CAN1:2:0 </param>
<param name="position"> atmotor atmotor </param>
<param name="resolution"> 0 0 </param>
<param name="tolerance"> 0 0 </param>
</group>

</group>

</group>

</group>

</params>
Loading

0 comments on commit de271a5

Please sign in to comment.