Skip to content
GitLab
Commit 68e3a52b authored by Olivier Stasse's avatar Olivier Stasse Committed by olivier stasse
Browse files

[tests/python] Add test-parameter-server.py

parent 555707cc
Hide whitespace changes
Inline Side-by-side
tests/python/CMakeLists.txt
View file @ 68e3a52b
......@@ -2,6 +2,7 @@ SET(${PROJECT_NAME}_PYTHON_TESTS
initialize-euler
matrix-util
op-point-modifier
test-parameter-server
)
FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
ADD_PYTHON_UNIT_TEST("py-${TEST}" "tests/python/${TEST}.py")
......
tests/python/parameter_server_conf.py 0 → 100644
View file @ 68e3a52b
NJ = 2
model_path = ['/integration_tests/openrobots/share/']
urdfFileName = "/integration_tests/openrobots/share/"+\
"simple_humanoid_description/urdf/"+\
"simple_humanoid.urdf"
ImuJointName = "imu_joint"
mapJointNameToID = {
'j1': 0,
'j2': 1,
}
mapJointLimits = {
0: [-0.349065850399, 1.57079632679],
1: [-0.5236, 0.5236],
}
vfMax = [100.0, 100.0]
vfMin = [-100.0, -100.0]
mapForceIdToForceLimits = {0: [vfMin, vfMax],
1: [vfMin, vfMax]}
mapNameToForceId = {"rf": 0, "lf": 1 }
indexOfForceSensors = ()
footFrameNames = {"Right": "RLEG_ANKLE_R", "Left": "LLEG_ANKLE_R"}
rightFootSensorXYZ = (0.0, 0.0, -0.085)
rightFootSoleXYZ = (0.0, 0.0, -0.105)
urdftosot = (0, 1)
tests/python/test-parameter-server.py 0 → 100644
View file @ 68e3a52b
import numpy as np
import pinocchio as pin
from numpy.testing import assert_almost_equal as assertApprox
# Switch pinocchio to numpy matrix
pin.switchToNumpyMatrix()
import parameter_server_conf as param_server_conf
from dynamic_graph.sot.core.parameter_server import ParameterServer
param_server = ParameterServer("param_server")
param_server.init(0.001,"rrbot.urdf","rrbot")
# Control time interval
dt=0.001
robot_name = 'robot'
# Create neutral pose for the robot
halfSitting = [
0.0,
0.0,
0.0,
0.0
]
# Initialize configuration vector
q = np.matrix(halfSitting).T
print("q:")
print(q.flatten().tolist()[0])
urdfPath= param_server_conf.urdfFileName
urdfDir= param_server_conf.model_path
# Read rrbot model
urdf_rrbot_model_string="<?xml version=\"1.0\" encoding=\"utf-8\"?>\
<!-- =================================================================================== -->\
<!-- | This document was autogenerated by xacro from /integration_tests/sot_bionic_ws/install/share/rrbot_description/urdf/rrbot.xacro | -->\
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->\
<!-- =================================================================================== -->\
<!-- Revolute-Revolute Manipulator -->\
<robot name=\"rrbot\">\
<!-- Space btw top of beam and the each joint -->\
<!-- ros_control plugin -->\
<gazebo>\
<plugin filename=\"libgazebo_ros_control.so\" name=\"gazebo_ros_control\">\
<robotNamespace>/rrbot</robotNamespace>\
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>\
</plugin>\
</gazebo>\
<!-- Link1 -->\
<gazebo reference=\"link1\">\
<material>Gazebo/Orange</material>\
</gazebo>\
<!-- Link2 -->\
<gazebo reference=\"link2\">\
<mu1>0.2</mu1>\
<mu2>0.2</mu2>\
<material>Gazebo/Black</material>\
</gazebo>\
<!-- Link3 -->\
<gazebo reference=\"link3\">\
<mu1>0.2</mu1>\
<mu2>0.2</mu2>\
<material>Gazebo/Orange</material>\
</gazebo>\
<!-- camera_link -->\
<gazebo reference=\"camera_link\">\
<mu1>0.2</mu1>\
<mu2>0.2</mu2>\
<material>Gazebo/Red</material>\
</gazebo>\
<!-- hokuyo -->\
<gazebo reference=\"hokuyo_link\">\
<sensor name=\"head_hokuyo_sensor\" type=\"gpu_ray\">\
<pose>0 0 0 0 0 0</pose>\
<visualize>false</visualize>\
<update_rate>40</update_rate>\
<ray>\
<scan>\
<horizontal>\
<samples>720</samples>\
<resolution>1</resolution>\
<min_angle>-1.570796</min_angle>\
<max_angle>1.570796</max_angle>\
</horizontal>\
</scan>\
<range>\
<min>0.10</min>\
<max>30.0</max>\
<resolution>0.01</resolution>\
</range>\
<noise>\
<type>gaussian</type>\
<!-- Noise parameters based on published spec for Hokuyo laser\
achieving \"+-30mm\" accuracy at range < 10m. A mean of 0.0m and\
stddev of 0.01m will put 99.7% of samples within 0.03m of the true\
reading. -->\
<mean>0.0</mean>\
<stddev>0.01</stddev>\
</noise>\
</ray>\
<plugin filename=\"libgazebo_ros_gpu_laser.so\" name=\"gazebo_ros_head_hokuyo_controller\">\
<topicName>/rrbot/laser/scan</topicName>\
<frameName>hokuyo_link</frameName>\
</plugin>\
</sensor>\
</gazebo>\
<!-- camera -->\
<gazebo reference=\"camera_link\">\
<sensor name=\"camera1\" type=\"camera\">\
<update_rate>30.0</update_rate>\
<camera name=\"head\">\
<horizontal_fov>1.3962634</horizontal_fov>\
<image>\
<width>800</width>\
<height>800</height>\
<format>R8G8B8</format>\
</image>\
<clip>\
<near>0.02</near>\
<far>300</far>\
</clip>\
<noise>\
<type>gaussian</type>\
<!-- Noise is sampled independently per pixel on each frame.\
That pixel's noise value is added to each of its color\
channels, which at that point lie in the range [0,1]. -->\
<mean>0.0</mean>\
<stddev>0.007</stddev>\
</noise>\
</camera>\
<plugin filename=\"libgazebo_ros_camera.so\" name=\"camera_controller\">\
<alwaysOn>true</alwaysOn>\
<updateRate>0.0</updateRate>\
<cameraName>rrbot/camera1</cameraName>\
<imageTopicName>image_raw</imageTopicName>\
<cameraInfoTopicName>camera_info</cameraInfoTopicName>\
<frameName>camera_link_optical</frameName>\
<!-- setting hackBaseline to anything but 0.0 will cause a misalignment\
between the gazebo sensor image and the frame it is supposed to\
be attached to -->\
<hackBaseline>0.0</hackBaseline>\
<distortionK1>0.0</distortionK1>\
<distortionK2>0.0</distortionK2>\
<distortionK3>0.0</distortionK3>\
<distortionT1>0.0</distortionT1>\
<distortionT2>0.0</distortionT2>\
<CxPrime>0</CxPrime>\
<Cx>0.0</Cx>\
<Cy>0.0</Cy>\
<focalLength>0.0</focalLength>\
</plugin>\
</sensor>\
</gazebo>\
<material name=\"black\">\
<color rgba=\"0.0 0.0 0.0 1.0\"/>\
</material>\
<material name=\"blue\">\
<color rgba=\"0.0 0.0 0.8 1.0\"/>\
</material>\
<material name=\"green\">\
<color rgba=\"0.0 0.8 0.0 1.0\"/>\
</material>\
<material name=\"grey\">\
<color rgba=\"0.2 0.2 0.2 1.0\"/>\
</material>\
<material name=\"orange\">\
<color rgba=\"1.0 0.423529411765 0.0392156862745 1.0\"/>\
</material>\
<material name=\"brown\">\
<color rgba=\"0.870588235294 0.811764705882 0.764705882353 1.0\"/>\
</material>\
<material name=\"red\">\
<color rgba=\"0.8 0.0 0.0 1.0\"/>\
</material>\
<material name=\"white\">\
<color rgba=\"1.0 1.0 1.0 1.0\"/>\
</material>\
<!-- Used for fixing robot to Gazebo 'base_link' -->\
<link name=\"world\"/>\
<joint name=\"fixed\" type=\"fixed\">\
<parent link=\"world\"/>\
<child link=\"link1\"/>\
</joint>\
<!-- Base Link -->\
<link name=\"link1\">\
<collision>\
<origin rpy=\"0 0 0\" xyz=\"0 0 1.0\"/>\
<geometry>\
<box size=\"0.1 0.1 2\"/>\
</geometry>\
</collision>\
<visual>\
<origin rpy=\"0 0 0\" xyz=\"0 0 1.0\"/>\
<geometry>\
<box size=\"0.1 0.1 2\"/>\
</geometry>\
<material name=\"orange\"/>\
</visual>\
<inertial>\
<origin rpy=\"0 0 0\" xyz=\"0 0 1.0\"/>\
<mass value=\"1\"/>\
<inertia ixx=\"0.334166666667\" ixy=\"0.0\" ixz=\"0.0\" iyy=\"0.334166666667\" iyz=\"0.0\" izz=\"0.00166666666667\"/>\
</inertial>\
</link>\
<joint name=\"joint1\" type=\"continuous\">\
<parent link=\"link1\"/>\
<child link=\"link2\"/>\
<origin rpy=\"0 0 0\" xyz=\"0 0.1 1.95\"/>\
<axis xyz=\"0 1 0\"/>\
<dynamics damping=\"0.7\"/>\
</joint>\
<!-- Middle Link -->\
<link name=\"link2\">\
<collision>\
<origin rpy=\"0 0 0\" xyz=\"0 0 0.45\"/>\
<geometry>\
<box size=\"0.1 0.1 1\"/>\
</geometry>\
</collision>\
<visual>\
<origin rpy=\"0 0 0\" xyz=\"0 0 0.45\"/>\
<geometry>\
<box size=\"0.1 0.1 1\"/>\
</geometry>\
<material name=\"black\"/>\
</visual>\
<inertial>\
<origin rpy=\"0 0 0\" xyz=\"0 0 0.45\"/>\
<mass value=\"1\"/>\
<inertia ixx=\"0.0841666666667\" ixy=\"0.0\" ixz=\"0.0\" iyy=\"0.0841666666667\" iyz=\"0.0\" izz=\"0.00166666666667\"/>\
</inertial>\
</link>\
<joint name=\"joint2\" type=\"continuous\">\
<parent link=\"link2\"/>\
<child link=\"link3\"/>\
<origin rpy=\"0 0 0\" xyz=\"0 0.1 0.9\"/>\
<axis xyz=\"0 1 0\"/>\
<dynamics damping=\"0.7\"/>\
</joint>\
<!-- Top Link -->\
<link name=\"link3\">\
<collision>\
<origin rpy=\"0 0 0\" xyz=\"0 0 0.45\"/>\
<geometry>\
<box size=\"0.1 0.1 1\"/>\
</geometry>\
</collision>\
<visual>\
<origin rpy=\"0 0 0\" xyz=\"0 0 0.45\"/>\
<geometry>\
<box size=\"0.1 0.1 1\"/>\
</geometry>\
<material name=\"orange\"/>\
</visual>\
<inertial>\
<origin rpy=\"0 0 0\" xyz=\"0 0 0.45\"/>\
<mass value=\"1\"/>\
<inertia ixx=\"0.0841666666667\" ixy=\"0.0\" ixz=\"0.0\" iyy=\"0.0841666666667\" iyz=\"0.0\" izz=\"0.00166666666667\"/>\
</inertial>\
</link>\
<joint name=\"hokuyo_joint\" type=\"fixed\">\
<axis xyz=\"0 1 0\"/>\
<origin rpy=\"0 0 0\" xyz=\"0 0 0.975\"/>\
<parent link=\"link3\"/>\
<child link=\"hokuyo_link\"/>\
</joint>\
<!-- Hokuyo Laser -->\
<link name=\"hokuyo_link\">\
<collision>\
<origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\
<geometry>\
<box size=\"0.1 0.1 0.1\"/>\
</geometry>\
</collision>\
<visual>\
<origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\
<geometry>\
<mesh filename=\"package://rrbot_description/meshes/hokuyo.dae\"/>\
</geometry>\
</visual>\
<inertial>\
<mass value=\"1e-5\"/>\
<origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\
<inertia ixx=\"1e-6\" ixy=\"0\" ixz=\"0\" iyy=\"1e-6\" iyz=\"0\" izz=\"1e-6\"/>\
</inertial>\
</link>\
<joint name=\"camera_joint\" type=\"fixed\">\
<axis xyz=\"0 1 0\"/>\
<origin rpy=\"0 0 0\" xyz=\"0.05 0 0.9\"/>\
<parent link=\"link3\"/>\
<child link=\"camera_link\"/>\
</joint>\
<!-- Camera -->\
<link name=\"camera_link\">\
<collision>\
<origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\
<geometry>\
<box size=\"0.05 0.05 0.05\"/>\
</geometry>\
</collision>\
<visual>\
<origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\
<geometry>\
<box size=\"0.05 0.05 0.05\"/>\
</geometry>\
<material name=\"red\"/>\
</visual>\
<inertial>\
<mass value=\"1e-5\"/>\
<origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\
<inertia ixx=\"1e-6\" ixy=\"0\" ixz=\"0\" iyy=\"1e-6\" iyz=\"0\" izz=\"1e-6\"/>\
</inertial>\
</link>\
<!-- generate an optical frame http://www.ros.org/reps/rep-0103.html#suffix-frames\
so that ros and opencv can operate on the camera frame correctly -->\
<joint name=\"camera_optical_joint\" type=\"fixed\">\
<!-- these values have to be these values otherwise the gazebo camera image\
won't be aligned properly with the frame it is supposedly originating from -->\
<origin rpy=\"-1.57079632679 0 -1.57079632679\" xyz=\"0 0 0\"/>\
<parent link=\"camera_link\"/>\
<child link=\"camera_link_optical\"/>\
</joint>\
<link name=\"camera_link_optical\">\
</link>\
<transmission name=\"tran1\">\
<type>transmission_interface/SimpleTransmission</type>\
<joint name=\"joint1\">\
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>\
</joint>\
<actuator name=\"motor1\">\
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>\
<mechanicalReduction>1</mechanicalReduction>\
</actuator>\
</transmission>\
<transmission name=\"tran2\">\
<type>transmission_interface/SimpleTransmission</type>\
<joint name=\"joint2\">\
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>\
</joint>\
<actuator name=\"motor2\">\
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>\
<mechanicalReduction>1</mechanicalReduction>\
</actuator>\
</transmission>\
</robot>\
"
#
model = pin.buildModelFromXML(urdf_rrbot_model_string)
print("model.nq:"+str(model.nq))
print(len(q))
neutral_configuration_model=pin.neutral(model)
#print(model)
jointIds=[]
reduced_model=pin.buildReducedModel(model, jointIds, q)
print(neutral_configuration_model)
print(len(q))
data = model.createData()
com = pin.centerOfMass(model, data, q)
pin.updateFramePlacements(model, data)
m = data.mass[0]
param_server.setParameter("urdf_model",urdf_rrbot_model_string)
model2_string=param_server.getParameter("urdf_model")
print(len(model2_string))
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment