Skip to content

Commit

Permalink
Merge pull request #57 from dqrobotics/20.04-hotfix1
Browse files Browse the repository at this point in the history 
20.04 hotfix1
  • Loading branch information
@bvadorno
bvadorno authored Jul 5, 2020
2 parents 203c833 + 2e987ae commit f11f0de
Show file tree
Hide file tree
Showing 5 changed files with 329 additions and 2 deletions.
4 changes: 2 additions & 2 deletions interfaces/DQ_VrepInterface.m → interfaces/vrep/DQ_VrepInterface.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -376,13 +376,13 @@ function set_object_pose(obj,handle,x,relative_to_handle,opmode)
r = rotation(x);
obj.set_object_translation(...
obj.handle_from_string_or_handle(handle),...
obj.handle_from_string_or_handle(relative_to_handle),...
t,...
obj.handle_from_string_or_handle(relative_to_handle),...
opmode);
obj.set_object_rotation(...
obj.handle_from_string_or_handle(handle),...
r,...
obj.handle_from_string_or_handle(relative_to_handle),...
r,...
opmode);
end
end
Expand Down
0 interfaces/DQ_VrepInterfaceMapElement.m → interfaces/vrep/DQ_VrepInterfaceMapElement.m
View file
File renamed without changes.
48 changes: 48 additions & 0 deletions interfaces/vrep/DQ_VrepRobot.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,48 @@
% CLASS DQ_VrepRobot - Abstract class with methods to send and receive
% robot information to and from VREP.
%
% Usage:
% Inherit from this class and implement the abstract methods.
%
% DQ_VrepRobot Methods:
% send_q_to_vrep - Sends the joint configurations to VREP
% get_q_from_vrep - Obtains the joint configurations from VREP
% kinematics - Obtains the DQ_Kinematics implementation of this
% robot

% (C) Copyright 2020 DQ Robotics Developers
%
% This file is part of DQ Robotics.
%
% DQ Robotics is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% DQ Robotics is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.
%
% DQ Robotics website: dqrobotics.sourceforge.net
%
% Contributors to this file:
% Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp

classdef (Abstract) DQ_VrepRobot

properties
robot_name
vrep_interface
end

methods (Abstract)
send_q_to_vrep(obj,q);
q = get_q_from_vrep(obj);
kin = kinematics(obj);
end
end

124 changes: 124 additions & 0 deletions interfaces/vrep/robots/LBR4pVrepRobot.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,124 @@
% CLASS LBR4pVrepRobot - Concrete class to interface with the "KUKA LBR4+"
% robot in VREP.
%
% Usage:
% 1) Drag-and-drop a "KUKA LBR4+" robot to a VREP scene.
% 2) Run
% >> vi = DQ_VrepInterface();
% >> vi.connect('127.0.0.1',19997);
% >> vrep_robot = LBR4pVrepRobot("LBR4p", vi);
% >> vi.start_simulation();
% >> robot.get_q_from_vrep();
% >> pause(1);
% >> vi.stop_simulation();
% >> vi.disconnect();
% Note that the name of the robot should be EXACTLY the same as in
% VREP. For instance, if you drag-and-drop a second robot, its name
% will become "LBR4p#0", a third robot, "LBR4p#1", and so on.
%
% LBR4pVrepRobot Methods:
% send_q_to_vrep - Sends the joint configurations to VREP
% get_q_from_vrep - Obtains the joint configurations from VREP
% kinematics - Obtains the DQ_Kinematics implementation of this robot

% (C) Copyright 2020 DQ Robotics Developers
%
% This file is part of DQ Robotics.
%
% DQ Robotics is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% DQ Robotics is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.
%
% DQ Robotics website: dqrobotics.sourceforge.net
%
% Contributors to this file:
% Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp

classdef LBR4pVrepRobot < DQ_VrepRobot

properties
joint_names;
base_frame_name;
end

methods
function obj = LBR4pVrepRobot(robot_name,vrep_interface)
%% Constructs an instance of a LBR4pVrepRobot
% >> vi = VrepInterface()
% >> vi.connect('127.0.0.1',19997);
% >> robot = LBR4pVrepRobot("LBR4p", vi)
obj.robot_name = robot_name;
obj.vrep_interface = vrep_interface;

% From the second copy of the robot and onward, VREP appends a
% #number in the robot's name. We check here if the robot is
% called by the correct name and assign an index that will be
% used to correctly infer the robot's joint labels.
splited_name = strsplit(robot_name,'#');
robot_label = splited_name{1};
if ~strcmp(robot_label,'LBR4p')
error('Expected LBR4p')
end
if length(splited_name) > 1
robot_index = splited_name{2};
else
robot_index = '';
end

% Initialize joint names and base frame
obj.joint_names = {};
for i=1:7
current_joint_name = {robot_label,'_joint',int2str(i),robot_index};
obj.joint_names{i} = strjoin(current_joint_name,'');
end
obj.base_frame_name = obj.joint_names{1};
end

function send_q_to_vrep(obj,q)
%% Sends the joint configurations to VREP
% >> vrep_robot = LBR4pVrepRobot("LBR4p", vi)
% >> q = zeros(7,1);
% >> vrep_robot.send_q_to_vrep(q)
obj.vrep_interface.set_joint_positions(obj.joint_names,q)
end

function q = get_q_from_vrep(obj)
%% Obtains the joint configurations from VREP
% >> vrep_robot = LBR4pVrepRobot("LBR4p", vi)
% >> q = vrep_robot.get_q_from_vrep(q)
q = obj.vrep_interface.get_joint_positions(obj.joint_names);
end

function kin = kinematics(obj)
%% Obtains the DQ_SerialManipulator instance that represents this LBR4p robot.
% >> vrep_robot = LBR4pVrepRobot("LBR4p", vi)
% >> robot_kinematics = vrep_robot.kinematics()

LBR4p_DH_theta=[0, 0, 0, 0, 0, 0, 0];
LBR4p_DH_d = [0.200, 0, 0.4, 0, 0.39, 0, 0];
LBR4p_DH_a = [0, 0, 0, 0, 0, 0, 0];
LBR4p_DH_alpha = [pi/2, -pi/2, pi/2, -pi/2, pi/2, -pi/2, 0];
LBR4p_DH_matrix = [LBR4p_DH_theta;
LBR4p_DH_d;
LBR4p_DH_a;
LBR4p_DH_alpha];

kin = DQ_SerialManipulator(LBR4p_DH_matrix,'standard');

kin.set_reference_frame(obj.vrep_interface.get_object_pose(obj.base_frame_name));
kin.set_base_frame(obj.vrep_interface.get_object_pose(obj.base_frame_name));
kin.set_effector(1+0.5*DQ.E*DQ.k*0.07);
end

end
end

155 changes: 155 additions & 0 deletions interfaces/vrep/robots/YouBotVrepRobot.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,155 @@
% CLASS YouBotVrepRobot - Concrete class to interface with the "KUKA YouBot"
% robot in VREP.
%
% Usage:
% 1) Drag-and-drop a "KUKA YouBot" robot to a VREP scene.
% 2) Run
% >> vi = DQ_VrepInterface();
% >> vi.connect('127.0.0.1',19997);
% >> vrep_robot = YouBotVrepRobot("youBot", vi);
% >> vi.start_simulation();
% >> robot.get_q_from_vrep();
% >> pause(1);
% >> vi.stop_simulation();
% >> vi.disconnect();
% Note that the name of the robot should be EXACTLY the same as in
% VREP. For instance, if you drag-and-drop a second robot, its name
% will become "youBot#0", a third robot, "youBot#1", and so on.
%
% YouBotVrepRobot Methods:
% send_q_to_vrep - Sends the joint configurations to VREP
% get_q_from_vrep - Obtains the joint configurations from VREP
% kinematics - Obtains the DQ_Kinematics implementation of this robot

% (C) Copyright 2020 DQ Robotics Developers
%
% This file is part of DQ Robotics.
%
% DQ Robotics is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% DQ Robotics is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with DQ Robotics. If not, see <http://www.gnu.org/licenses/>.
%
% DQ Robotics website: dqrobotics.sourceforge.net
%
% Contributors to this file:
% Murilo Marques Marinho - murilo@nml.t.u-tokyo.ac.jp

classdef YouBotVrepRobot < DQ_VrepRobot

properties
joint_names;
base_frame_name;
end

properties (Constant)
adjust = ((cos(pi/2) + DQ.i*sin(pi/2)) * (cos(pi/4) + DQ.j*sin(pi/4)))*(1+0.5*DQ.E*-0.1*DQ.k);
end

methods
function obj = YouBotVrepRobot(robot_name,vrep_interface)
%% Constructs an instance of a YouBotVrepRobot
% >> vi = VrepInterface()
% >> vi.connect('127.0.0.1',19997);
% >> robot = YouBotVrepRobot("youBot", vi)
obj.robot_name = robot_name;
obj.vrep_interface = vrep_interface;

% From the second copy of the robot and onward, VREP appends a
% #number in the robot's name. We check here if the robot is
% called by the correct name and assign an index that will be
% used to correctly infer the robot's joint labels.
splited_name = strsplit(robot_name,'#');
robot_label = splited_name{1};
if ~strcmp(robot_label,'youBot')
error('Expected youBot')
end
if length(splited_name) > 1
robot_index = splited_name{2};
else
robot_index = '';
end

% Initialize joint names and base frame
obj.joint_names = {};
for i=1:5
current_joint_name = {robot_label,'ArmJoint',int2str(i-1),robot_index};
obj.joint_names{i} = strjoin(current_joint_name,'');
end
obj.base_frame_name = robot_name;
end

function send_q_to_vrep(obj,q)
%% Sends the joint configurations to VREP
% >> vrep_robot = YouBotVrepRobot("youBot", vi)
% >> q = zeros(8,1);
% >> vrep_robot.send_q_to_vrep(q)
x = q(1);
y = q(2);
phi = q(3);

r = cos(phi/2.0)+DQ.k*sin(phi/2.0);
p = x * DQ.i + y * DQ.j;
pose = (1 + DQ.E*0.5*p)*r;

obj.vrep_interface.set_joint_positions(obj.joint_names,q(4:8));
obj.vrep_interface.set_object_pose(obj.base_frame_name, pose * obj.adjust');
end

function q = get_q_from_vrep(obj)
%% Obtains the joint configurations from VREP
% >> vrep_robot = YouBotVrepRobot("youBot", vi)
% >> q = vrep_robot.get_q_from_vrep(q)
base_x = obj.vrep_interface.get_object_pose(obj.base_frame_name) * obj.adjust;
base_t = vec3(translation(base_x));
base_phi = rotation_angle(rotation(base_x));
base_arm_q = obj.vrep_interface.get_joint_positions(obj.joint_names);

q = [base_t(1); base_t(2); base_phi; base_arm_q];
end

function kin = kinematics(obj)
%% Obtains the DQ_WholeBody instance that represents this youBot robot.
% >> vrep_robot = YouBotVrepRobot("youBot", vi)
% >> robot_kinematics = vrep_robot.kinematics()

include_namespace_dq
% The DH parameters and other geometric parameters are based on
% Kuka's documentation:
% http://www.youbot-store.com/wiki/index.php/YouBot_Detailed_Specifications
% https://www.generationrobots.com/img/Kuka-YouBot-Technical-Specs.pdf
pi2 = pi/2;
arm_DH_theta = [ 0, pi2, 0, pi2, 0];
arm_DH_d = [ 0.147, 0, 0, 0, 0.218];
arm_DH_a = [ 0.033, 0.155, 0.135, 0, 0];
arm_DH_alpha = [pi2, 0, 0, pi2, 0];
arm_DH_matrix = [arm_DH_theta;
arm_DH_d;
arm_DH_a;
arm_DH_alpha];

arm = DQ_SerialManipulator(arm_DH_matrix,'standard');
base = DQ_HolonomicBase();

x_bm = 1 + E_*0.5*(0.165*i_ + 0.11*k_);

base.set_frame_displacement(x_bm);

kin = DQ_WholeBody(base);
kin.add(arm);

effector = 1 + E_*0.5*0.3*k_;
kin.set_effector(effector);
end

end
end

0 comments on commit f11f0de

Please sign in to comment.