From 4360e5e3e4407843bad4742caa4bcd9af6b07a52 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Sun, 1 May 2016 19:40:50 +0200
Subject: [PATCH] Chnaged the rang of the simulated gyro to coincide with the
real one (from 1023 to 511). Added rviz and gazebo models for the feet
sensors (support board and cny70 sensor).
---
.../include/bioloid_controller_cm510_impl.h | 4 +-
.../meshes/bounding_boxes/cny70_bb.stl | Bin 0 -> 684 bytes
bioloid_description/meshes/cny70.stl | Bin 0 -> 3884 bytes
.../meshes/left_foot_board.stl | Bin 0 -> 1284 bytes
.../meshes/right_foot_board.stl | Bin 0 -> 1284 bytes
bioloid_description/urdf/bioloid_ceabot.xacro | 2 +
bioloid_description/urdf/cny70_ir.gazebo | 55 +++++++++++
bioloid_description/urdf/cny70_ir.xacro | 38 ++++++++
bioloid_description/urdf/feet_ir.xacro | 91 ++++++++++++++++++
9 files changed, 188 insertions(+), 2 deletions(-)
create mode 100644 bioloid_description/meshes/bounding_boxes/cny70_bb.stl
create mode 100644 bioloid_description/meshes/cny70.stl
create mode 100644 bioloid_description/meshes/left_foot_board.stl
create mode 100644 bioloid_description/meshes/right_foot_board.stl
create mode 100644 bioloid_description/urdf/cny70_ir.gazebo
create mode 100644 bioloid_description/urdf/cny70_ir.xacro
create mode 100644 bioloid_description/urdf/feet_ir.xacro
diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
index 6468d9a..7ccba10 100644
--- a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
+++ b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
@@ -354,12 +354,12 @@ namespace bioloid_controller_cm510
fb_imu_value=300.0;
else if(fb_imu_value<-300.0)
fb_imu_value=-300.0;
- adc_values[BALANCE_GYRO_X_CHANNEL]=((fb_imu_value*512)/300.0)+512;
+ adc_values[BALANCE_GYRO_X_CHANNEL]=((fb_imu_value*256)/300.0)+256;
if(lr_imu_value>300.0)
lr_imu_value=300.0;
else if(lr_imu_value<-300.0)
lr_imu_value=-300.0;
- adc_values[BALANCE_GYRO_Y_CHANNEL]=((lr_imu_value*512)/300.0)+512;
+ adc_values[BALANCE_GYRO_Y_CHANNEL]=((lr_imu_value*256)/300.0)+256;
}
else
{
diff --git a/bioloid_description/meshes/bounding_boxes/cny70_bb.stl b/bioloid_description/meshes/bounding_boxes/cny70_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..e3ef1eb5536cbc62de80c30f3b56d6c132d32e75
GIT binary patch
literal 684
zcmXTU&&f<tNY1M?H;7M4QlJ>{Fu*{=eh|wkAyp5EcSA9ZZM|&v5t!I+R2|4NNV<`A
zSi^KMG%zqU?1!qdMzdo#)FiMtst#lsBs*X_z%GO7Xs`#dkn8}N2l5BlTqKj=u7c=5
zmcgL|?sJe04faq~Als4c!0vOf4yYZtbbw3&y9(kWkS)kAL1M$~1DOPN6^0J53=SO-
eJHYV`vIE)YNNnWL#jXQn2h6u17a{8ax(Wc89L^d5
literal 0
HcmV?d00001
diff --git a/bioloid_description/meshes/cny70.stl b/bioloid_description/meshes/cny70.stl
new file mode 100644
index 0000000000000000000000000000000000000000..069b736f6734e8ae653bd8bfc6451426628da060
GIT binary patch
literal 3884
zcmb`JF>Vw=5JflO4kQj75h60AwZvM81c`_fSONkh1OXA*9)KGdQfI_SUL+H60j|L*
zsQ;^eb@g<&M8HTpE>HLC|EsEdc0Yc2_xAPqtM{KCKRW*(*NYvl;~j2@ZmxfwUOxZ2
ze{%SItN7lxpNel0V_r|(|Na}q9t2{W#rWyfn+tqK!ZdD0AQk~_#3Nzrc7z<p^NA~|
z!&Ss5IfV99BlDOL6OC==auqe4Yh<>jL1$5EJ|A7x`cxtuE~=}FYF210s;jz}T3x7C
z%YL9%+8Nt;lDTRkCxu4llj?2=&JsO$Rr9d864*Tp4b=)nb!G8956t2YuY`8!*g=d)
zL$jjN?EgsAK35{*Ngfk|y@}_irH0N#W*%-Q8XAcnWOe^dW7CiQyysf0^jUe}T{6#u
zBU{4Ut~HX-EYOIxOR`=&Q++5`CFc3ibIiuY_afh-8d(rLwa$QwS#(th_Qw9t%o_o}
z8krCnW3A}wfP2isTKFI*{H+@4>PVt!tccWbomlr2YfGcnDm9!4jTHf167Fb|2dLC2
z+H^G#n7ty>l{)zBN9=Yx=N{|K>p8sO62biw=X@meE^)@F1J;c*ks5kS=(%Q7#|VBq
zcN!uFSBKr@*^M;Zd)4!msw=A(i8dly8dFzyeSKV`)~aeS4<o!*)s+Znh^W6Zs`skV
z`s8cd{fJ1e-1|sK19O@AgOaViagyNMCo<<du@c#jDPbB(kjh$_t0X+m-7^qvM6N^(
zuJAi9GlzPq0`tHwsjm9HyCR^6m36hfTM3VI_Y6cEkt-3^Rp0a2)tQI%YUVwmkzS*9
zGjq7xUHwtd`PPr>@;f6=+V<Ok1oK!D^IBOR9Z@pxG~f=^WzVOC<<}9|yI*+Dq2W7&
zXCi1{sk#!;Mxs_70f(v;U2*S1y=r*u6{#8`+K4nn$gR(``w@|5sn43F-pJL^^Q_gD
zsIH8<Jj<Tj$iA#=ak|~Z3oa3DWJZ{)BuH5Yy$4mp<E;8Xv=M2Dh^i-{4rZU3M`X@C
z5LZ_sTmvJ$!=}x&<95uRSHfejNY!BGMzj%Wh^Vgm4mR(7PBrtM&`7Vra~_%h1xd=u
Ad;kCd
literal 0
HcmV?d00001
diff --git a/bioloid_description/meshes/left_foot_board.stl b/bioloid_description/meshes/left_foot_board.stl
new file mode 100644
index 0000000000000000000000000000000000000000..54942f0efd0396079f68821d4a7f869a8c41d6e1
GIT binary patch
literal 1284
zcmbV~u}T9$5QY~^J1Yg7K%PKMp^$Lk8`xP0QAq-Uz=27VZt-1gZ3QjT<ONb$Sc;tw
z;r%;&$?UCKxW(P>%zX3D?9RNNO-G&Cc(NEy>UuG}t8ec|o&T?+pcSk1Yd+og<=}LE
z@^o?Y`O5z4yw}(M@Okqhdr9E=i4dK#$Mg+xwg1*FG?Z6%GL-}#G^h}}h`2J15YW(>
zz1JWvCzR_qg3d!U)=1rzC(IWDnbJ9$)d)OPB_EwzobN*xX?#2$57-l>OxSmQ$m=~+
zh#i`LtiCSK(tD5#J=uMxobVn9G%V9W|2oap^kJ{!)ie!WkGZlbX%o_D6Gc^#`Smxd
zN_^+OLcO3(JXJ9niej#`_p_T5@}TEw>82Ii8lF&uzlEIe9yC%!^U3;X&bxQlT`8(P
qOa7hY&k4PhCyLwM5=3!7%1h^HE5DX$K*+*7dwP4je-etytNI4==5S>I
literal 0
HcmV?d00001
diff --git a/bioloid_description/meshes/right_foot_board.stl b/bioloid_description/meshes/right_foot_board.stl
new file mode 100644
index 0000000000000000000000000000000000000000..567d6bb539f501c27a133637d071f0c4a9060b51
GIT binary patch
literal 1284
zcmbW0u}T9$5QY~^I}wavVIZxIXb~Y2@CDS)LWoLG4}^o5Cf#A-a|Bed5Wz+vB0<Ot
zq_QxLh>iFV-gnE-&1E?Yx44_k%s2n+&Ez~ji`vyrbaL8l9mjFEbrc_V+SPxbMd#dO
z<!&K;r6(EAzSUmVMyvN9+Yj@2uv7mf{>syM;QiA?9?1!4{Lt7*lV`oRQj^Bk!Pm3w
z<<X=s4g4WNtug}Ix!3R8YdG#vd+%lUumm=KRSF43)Rk!YeQloUJ+7p&x3S)kZ8h+R
z1hvWtXy@h{`$M?sVy5}CThH$Cd9$=B4QTvS!XFZfSOQzxzJD~B%g$ijXU^2JdiYf-
zBowhku`4@sl>9C^XZ7$?i8m+T5`HOSiDFk|iNcbGCdj^8njDVTu@OtyJNbMO%F>+C
z)0SZCf0Jv7AfkC#T3hlrp_!xMXYTt-*pvhfzLnJNcf$?|ZG(CzOI=|Xy2>;}5Rsmx
VrJdcQlpt$*OLD?SqybT!`7hU=*D(M9
literal 0
HcmV?d00001
diff --git a/bioloid_description/urdf/bioloid_ceabot.xacro b/bioloid_description/urdf/bioloid_ceabot.xacro
index 474d40e..2c1d925 100755
--- a/bioloid_description/urdf/bioloid_ceabot.xacro
+++ b/bioloid_description/urdf/bioloid_ceabot.xacro
@@ -2,6 +2,7 @@
<xacro:include filename="$(find bioloid_description)/urdf/bioloid.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/sharp_ir.xacro" />
+ <xacro:include filename="$(find bioloid_description)/urdf/feet_ir.xacro" />
<xacro:sharp_ir name="IR1" parent="base_link" update_rate="100" fov="0.05" min_range="0.1" max_range="0.8">
<origin xyz="0 0.0 0.035" rpy="-1.5707 -1.5707 0" />
@@ -9,5 +10,6 @@
<xacro:sharp_ir name="IR2" parent="base_link" update_rate="100" fov="0.05" min_range="0.1" max_range="0.8">
<origin xyz="0 -0.05 0.035" rpy="-1.5707 -1.5707 0" />
</xacro:sharp_ir>
+ <xacro:feet_ir left_parent="left_leg_ankle_roll" right_parent="right_leg_ankle_roll" update_rate="100" range="0.005"/>
</robot>
diff --git a/bioloid_description/urdf/cny70_ir.gazebo b/bioloid_description/urdf/cny70_ir.gazebo
new file mode 100644
index 0000000..a0b2b85
--- /dev/null
+++ b/bioloid_description/urdf/cny70_ir.gazebo
@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:macro name="cny70_ir_gazebo" params="name update_rate fov min_range max_range">
+ <gazebo reference="${name}_ir_link">
+ <material>Gazebo/Black</material>
+ <gravity>true</gravity>
+ <self_collide>false</self_collide>
+ <mu1>0.200000</mu1>
+ <mu2>0.200000</mu2>
+ </gazebo>
+
+ <gazebo reference="${name}_ir_joint">
+ <implicitSpringDamper>true</implicitSpringDamper>
+ </gazebo>
+
+ <gazebo reference="${name}_ir_link">
+ <sensor type="ray" name="${name}">
+ <always_on>true</always_on>
+ <update_rate>${update_rate}</update_rate>
+ <visualize>false</visualize>
+ <ray>
+ <scan>
+ <horizontal>
+ <samples>1</samples>
+ <resolution>1</resolution>
+ <min_angle>-${fov/2}</min_angle>
+ <max_angle>${fov/2}</max_angle>
+ </horizontal>
+ <vertical>
+ <samples>1</samples>
+ <resolution>1</resolution>
+ <min_angle>-${fov/2}</min_angle>
+ <max_angle>${fov/2}</max_angle>
+ </vertical>
+ </scan>
+ <range>
+ <min>${min_range}</min>
+ <max>${max_range}</max>
+ <resolution>0.001</resolution>
+ </range>
+ </ray>
+ <plugin name="${name}_range_sensor" filename="libgazebo_ros_range.so">
+ <frameName>${name}_ir_link</frameName>
+ <topicName>/bioloid/sensors/range</topicName>
+ <radiation>infrared</radiation>
+ <fov>${fov}</fov>
+ <gaussianNoise>0.01</gaussianNoise>
+ <updateRate>${update_rate}</updateRate>
+ </plugin>
+ </sensor>
+ </gazebo>
+ </xacro:macro>
+</root>
diff --git a/bioloid_description/urdf/cny70_ir.xacro b/bioloid_description/urdf/cny70_ir.xacro
new file mode 100644
index 0000000..1798773
--- /dev/null
+++ b/bioloid_description/urdf/cny70_ir.xacro
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find bioloid_description)/urdf/cny70_ir.gazebo" />
+
+ <xacro:macro name="cny70_ir" params="name parent *origin update_rate fov min_range max_range">
+ <!-- IR distance sensors -->
+ <link name="${name}_ir_link">
+ <collision>
+ <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://bioloid_description/meshes/bounding_boxes/cny70_bb.stl" />
+ </geometry>
+ </collision>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://bioloid_description/meshes/cny70.stl"/>
+ </geometry>
+ <material name="black"/>
+ </visual>
+ <inertial>
+ <mass value="0.0007" />
+ <origin xyz="-0.00307736 0.0000000 0.00000000" rpy="0 0 0"/>
+ <inertia ixx="0.000000001" ixy="0.00000000" ixz="0.000000" iyy="0.000000001" iyz="0.00000000" izz="0.000000001" />
+ </inertial>
+ </link>
+ <joint name="${name}_ir_joint" type="fixed">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}_ir_link"/>
+ </joint>
+
+ <xacro:cny70_ir_gazebo name="${name}" update_rate="${update_rate}" fov="${fov}" min_range="${min_range}" max_range="${max_range}"/>
+ </xacro:macro>
+</root>
+
diff --git a/bioloid_description/urdf/feet_ir.xacro b/bioloid_description/urdf/feet_ir.xacro
new file mode 100644
index 0000000..04dc1af
--- /dev/null
+++ b/bioloid_description/urdf/feet_ir.xacro
@@ -0,0 +1,91 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find bioloid_description)/urdf/cny70_ir.xacro" />
+
+ <xacro:macro name="feet_ir" params="left_parent right_parent update_rate range">
+ <!-- IR distance sensors -->
+ <link name="left_board_link">
+ <collision>
+ <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://bioloid_description/meshes/left_foot_board.stl" />
+ </geometry>
+ </collision>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://bioloid_description/meshes/left_foot_board.stl"/>
+ </geometry>
+ <material name="green"/>
+ </visual>
+ <inertial>
+ <mass value="0.011" />
+ <origin xyz="0.02939017 -0.00080000 0.01656892" rpy="0 0 0"/>
+ <inertia ixx="0.00000152" ixy="0.00000000" ixz="-0.00000061" iyy="0.00000465" iyz="0.00000000" izz="0.00000313" />
+ </inertial>
+ </link>
+
+ <joint name="left_board_joint" type="fixed">
+ <origin xyz="-0.024 -0.0179 0.024" rpy="0 0 0"/>
+ <parent link="${left_parent}"/>
+ <child link="left_board_link"/>
+ </joint>
+
+ <!-- sensor links -->
+ <xacro:cny70_ir name="left_front_fwd" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
+ <origin xyz="0.0085 -0.0055 0.038" rpy="-1.5707 -1.5707 0" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="left_front_dwn" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
+ <origin xyz="0.0225 -0.012 0.038" rpy="0 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="left_lateral_dwn" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
+ <origin xyz="0.0575 -0.012 0.0065" rpy="0 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <link name="right_board_link">
+ <collision>
+ <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://bioloid_description/meshes/right_foot_board.stl" />
+ </geometry>
+ </collision>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://bioloid_description/meshes/right_foot_board.stl"/>
+ </geometry>
+ <material name="green"/>
+ </visual>
+ <inertial>
+ <mass value="0.011" />
+ <origin xyz="-0.02939017 -0.00080000 0.01656892" rpy="0 0 0"/>
+ <inertia ixx="0.00000152" ixy="0.00000000" ixz="0.00000061" iyy="0.00000465" iyz="0.00000000" izz="0.00000313" />
+ </inertial>
+ </link>
+
+ <joint name="right_board_joint" type="fixed">
+ <origin xyz="0.024 -0.0179 0.024" rpy="0 0 0"/>
+ <parent link="${right_parent}"/>
+ <child link="right_board_link"/>
+ </joint>
+
+ <!-- sensor links -->
+ <xacro:cny70_ir name="right_front_fwd" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
+ <origin xyz="-0.0085 -0.0055 0.038" rpy="-1.5707 -1.5707 0" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="right_front_dwn" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
+ <origin xyz="-0.0225 -0.012 0.038" rpy="0 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="right_lateral_dwn" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
+ <origin xyz="-0.0575 -0.012 0.0065" rpy="0 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ </xacro:macro>
+</root>
+
--
GitLab