Unverified Commit 2ecd7a4d authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub

Merge pull request #73 from pFernbach/devel

Update API to load robots
parents e7d6ad6c c3fe7e60
......@@ -32,21 +32,14 @@ module hpp
/// This function can be called several times to include several ROMs (one for each limb)
/// The device automatically has an anchor joint called "base_joint" as
/// root joint.
/// \param romRobotName the name of the robot range of motion.
/// Load robot model
/// \param robotName the name of the robot range of motion,
/// \param rootJointType type of root joint among "anchor", "freeflyer",
/// "planar",
/// \param packageName Name of the ROS package containing the model,
/// \param modelName Name of the package containing the model
/// \param urdfSuffix suffix for urdf file,
/// \param urdfName name of the urdf file. It may contain
/// "file://", or "package://" prefixes.
///
/// The ros url are built as follows:
/// "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
/// "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
///
void loadRobotRomModel (in string romRobotName, in string rootJointType,
in string packageName, in string modelName,
in string urdfSuffix, in string srdfSuffix)
void loadRobotRomModel (in string robotName, in string rootJointType,
in string urdfName)
raises (Error);
......@@ -54,39 +47,34 @@ module hpp
/// The device automatically has an anchor joint called "base_joint" as
/// root joint.
/// \param trunkRobotName the name of the robot trunk used for collision.
/// \param robotName the name of the robot trunk used for collision,
/// \param rootJointType type of root joint among "anchor", "freeflyer",
/// "planar",
/// \param packageName Name of the ROS package containing the model,
/// \param modelName Name of the package containing the model
/// \param urdfSuffix suffix for urdf file,
///
/// The ros url are built as follows:
/// "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
/// "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
/// \param urdfName name of the urdf file. It may contain
/// "file://", or "package://" prefixes.
/// \param srdfName name of the srdf file. It may contain
/// "file://", or "package://" prefixes.
///
void loadRobotCompleteModel (in string trunkRobotName, in string rootJointType,
in string packageName, in string modelName,
in string urdfSuffix, in string srdfSuffix)
void loadRobotCompleteModel (in string robotName, in string rootJointType,
in string urdfName, in string srdfName)
raises (Error);
/// Load fullbody robot model
///
/// Create a RbprmFullBody object
/// The device automatically has an anchor joint called "base_joint" as
/// root joint.
/// \param trunkRobotName the name of the robot trunk used for collision.
///
/// \param robotName Name of the robot that is constructed,
/// \param rootJointType type of root joint among "anchor", "freeflyer",
/// "planar",
/// \param packageName Name of the ROS package containing the model,
/// \param modelName Name of the package containing the model
/// \param urdfSuffix suffix for urdf file,
///
/// The ros url are built as follows:
/// "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
/// "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"
/// \param urdfName name of the urdf file. It may contain
/// "file://", or "package://" prefixes.
/// \param srdfName name of the srdf file. It may contain
/// "file://", or "package://" prefixes.
///
void loadFullBodyRobot (in string trunkRobotName, in string rootJointType,
in string packageName, in string modelName,
in string urdfSuffix, in string srdfSuffix, in string selectedProblem)
void loadFullBodyRobot (in string robotName, in string rootJointType,
in string urdfName, in string srdfName, in string selectedProblem)
raises (Error);
/// Create a RbprmFullBody object
......
% Open it on blender with default axis setting and export it with x forward.
% Use decimate to reduce the number of faces, don't forget to triangulate faces before exporting
function [] = effectorRomToObj(filename, outname)
delimiterIn = ',';
headerlinesIn = 0;
points = importdata(filename,delimiterIn,headerlinesIn);
k = convhull(points);
vertface2obj(points,k,outname)
function vertface2obj(v,f,name)
% VERTFACE2OBJ Save a set of vertice coordinates and faces as a Wavefront/Alias Obj file
% VERTFACE2OBJ(v,f,fname)
% v is a Nx3 matrix of vertex coordinates.
% f is a Mx3 matrix of vertex indices.
% fname is the filename to save the obj file.
fid = fopen(name,'w');
for i=1:size(v,1)
fprintf(fid,'v %f %f %f\n',v(i,1),v(i,2),v(i,3));
end
fprintf(fid,'g foo\n');
for i=1:size(f,1);
fprintf(fid,'f %d %d %d\n',f(i,1),f(i,2),f(i,3));
end
fprintf(fid,'g\n');
fclose(fid);
......@@ -88,6 +88,7 @@ SET(${PROJECT_NAME}_PYTHON_FILES
SET(${PROJECT_NAME}_PYTHON_TOOLS
affordance_centroids.py
com_constraints.py
constants_and_tools.py
constraint_to_dae.py
cwc_trajectory_helper.py
cwc_trajectory.py
......
......@@ -26,42 +26,55 @@ from hpp.corbaserver.robot import Robot
# trunk of the robot, and a set of robots describe the range of motion of each limb of the robot.
class Builder(Robot):
# # Constructor
def __init__(self, load=True, clientRbprm=None):
def __init__(self, robotName = None, rootJointType = None, load = True, client = None, hppcorbaClient = None, clientRbprm=None):
Robot.__init__(self, robotName, rootJointType, False, client, hppcorbaClient)
self.tf_root = "base_link"
if clientRbprm is None:
self.clientRbprm = RbprmClient()
else:
self.clientRbprm = clientRbprm
self.load = load
if load:
self.loadModel(robotName, rootJointType)
## Return urdf and srdf filenames of the roms
#
def urdfROMFilenames (self):
urdfNameRoms = []
nameRoms = []
if hasattr (self, 'packageName') and hasattr (self, 'urdfNameRom') and \
hasattr (self, 'urdfSuffix') :
if not isinstance(self.urdfNameRom, list):
self.urdfNameRom = [self.urdfNameRom]
nameRoms = self.urdfNameRom
for urdfName in nameRoms:
urdfNameRoms += ["package://" + self.packageName + '/urdf/' + urdfName + self.urdfSuffix + '.urdf']
elif hasattr (self, 'urdfFilenameRom'):
urdfNameRoms = self.urdfFilenameRom
for urdfNameRom in urdfNameRoms:
nameRoms += [urdfNameRom.split("/")[-1].rstrip(".urdf")]
else :
raise RuntimeError (\
"""instance should have one of the following sets of members
- (packageName, urdfNameRom, urdfSuffix),
- (urdfFilenameRom)""")
return nameRoms, urdfNameRoms
# # Virtual function to load the robot model.
#
# \param urdfName urdf description of the robot trunk,
# \param urdfNameroms either a string, or an array of strings, indicating the urdf of the different roms to add.
# This method looks for the following class attribute in order to find the files to load:
# First it looks for urdfFilename and srdfFilename and use it if available
# Otherwise it looks for packageName, urdfName, urdfSuffix, srdfSuffix
# \param robotNamethe name of the robot
# \param rootJointType type of root joint among ("freeflyer", "planar",
# "anchor"),
# \param meshPackageName name of the meshpackage from where the robot mesh will be loaded
# \param packageName name of the package from where the robot will be loaded
# \param urdfSuffix optional suffix for the urdf of the robot package
# \param srdfSuffix optional suffix for the srdf of the robot package
def loadModel(self, urdfName, urdfNameroms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix, client=None):
Robot.__init__(self, urdfName, rootJointType, False, client=client)
if (isinstance(urdfNameroms, list)):
for urdfNamerom in urdfNameroms:
self.clientRbprm.rbprm.loadRobotRomModel(urdfNamerom, rootJointType, packageName, urdfNamerom,
urdfSuffix, srdfSuffix)
else:
self.clientRbprm.rbprm.loadRobotRomModel(urdfNameroms, rootJointType, packageName, urdfNameroms,
urdfSuffix, srdfSuffix)
# "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots
def loadModel(self, robotName, rootJointType):
nameRoms, urdfROMFilenames = self.urdfROMFilenames()
for i, urdfNameRom in enumerate(urdfROMFilenames):
self.clientRbprm.rbprm.loadRobotRomModel(nameRoms[i], rootJointType, urdfNameRom)
self.clientRbprm.rbprm.initNewProblemSolver()
self.clientRbprm.rbprm.loadRobotCompleteModel(urdfName, rootJointType, packageName, urdfName, urdfSuffix,
srdfSuffix)
self.client.robot.meshPackageName = meshPackageName
self.meshPackageName = meshPackageName
self.packageName = packageName
self.urdfName = urdfName
self.urdfSuffix = urdfSuffix
self.srdfSuffix = srdfSuffix
urdfFilename, srdfFilename = self.urdfSrdfFilenames () # inherited method from corbaserver.robot
self.clientRbprm.rbprm.loadRobotCompleteModel(robotName, rootJointType, urdfFilename, srdfFilename)
# # Init RbprmShooter
#
......
......@@ -28,7 +28,8 @@ from numpy import array, matrix
# trunk of the robot, one for the range of motion
class FullBody(Robot):
# # Constructor
def __init__(self, load=True, clientRbprm=None):
def __init__(self, robotName = None, rootJointType = None, load = True, client = None, hppcorbaClient = None, clientRbprm=None):
Robot.__init__(self, robotName, rootJointType, False, client, hppcorbaClient)
self.tf_root = "base_link"
if clientRbprm == None:
self.clientRbprm = RbprmClient()
......@@ -36,27 +37,22 @@ class FullBody(Robot):
self.clientRbprm = clientRbprm
self.load = load
self.limbNames = []
if load:
self.loadFullBodyModel(robotName, rootJointType)
# # Virtual function to load the fullBody robot model.
#
# \param urdfName urdf description of the fullBody robot
# This method looks for the following class attribute in order to find the files to load:
# First it looks for urdfFilename and srdfFilename and use it if available
# Otherwise it looks for packageName, urdfName, urdfSuffix, srdfSuffix
# \param robotNamethe name of the robot
# \param rootJointType type of root joint among ("freeflyer", "planar",
# "anchor"), WARNING. Currently RB-PRM only considerds freeflyer roots
# \param meshPackageName name of the meshpackage from where the robot mesh will be loaded
# \param packageName name of the package from where the robot will be loaded
# \param urdfSuffix optional suffix for the urdf of the robot package
# \param srdfSuffix optional suffix for the srdf of the robot package
def loadFullBodyModel(self, urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix, client=None):
Robot.__init__(self, urdfName, rootJointType, False, client)
self.clientRbprm.rbprm.loadFullBodyRobot(urdfName, rootJointType, packageName, urdfName, urdfSuffix,
srdfSuffix,
def loadFullBodyModel(self, robotName, rootJointType):
urdfFilename, srdfFilename = self.urdfSrdfFilenames () # inherited method from corbaserver.robot
print("selected problem: ",self.client.problem.getSelected("problem")[0])
self.clientRbprm.rbprm.loadFullBodyRobot(robotName, rootJointType,
urdfFilename, srdfFilename,
self.client.problem.getSelected("problem")[0])
self.client.robot.meshPackageName = meshPackageName
self.meshPackageName = meshPackageName
self.packageName = packageName
self.urdfName = urdfName
self.urdfSuffix = urdfSuffix
self.srdfSuffix = srdfSuffix
self.octrees = {}
# Virtual function to load the fullBody robot model.
......
from abc import abstractmethod
from hpp.gepetto import ViewerFactory, PathPlayer
from hpp.corbaserver.affordance.affordance import AffordanceTool
from hpp.corbaserver import ProblemSolver
from hpp.corbaserver import ProblemSolver, Client
from hpp.corbaserver import createContext, loadServerPlugin
from hpp.corbaserver.rbprm import Client as RbprmClient
class AbstractPathPlanner:
......@@ -13,7 +15,11 @@ class AbstractPathPlanner:
extra_dof_bounds = None
robot_node_name = None # name of the robot in the node list of the viewer
def __init__(self):
def __init__(self, context = None):
"""
Constructor
:param context: An optional string that give a name to a corba context instance
"""
self.v_max = -1 # bounds on the linear velocity for the root, negative values mean unused
self.a_max = -1 # bounds on the linear acceleration for the root, negative values mean unused
self.root_translation_bounds = [0] * 6 # bounds on the root translation position (-x, +x, -y, +y, -z, +z)
......@@ -27,6 +33,17 @@ class AbstractPathPlanner:
self.size_foot_y = 0 # size of the feet along the y axis
self.q_init = []
self.q_goal = []
self.context = context
if context:
createContext(context)
loadServerPlugin(context, 'rbprm-corba.so')
loadServerPlugin(context, 'affordance-corba.so')
self.hpp_client = Client(context=context)
self.hpp_client.problem.selectProblem(context)
self.rbprm_client = RbprmClient(context=context)
else:
self.hpp_client = None
self.rbprm_client = None
@abstractmethod
def load_rbprm(self):
......@@ -107,7 +124,11 @@ class AbstractPathPlanner:
:param visualize_affordances: list of affordances type to visualize, default to none
"""
vf = ViewerFactory(self.ps)
self.afftool = AffordanceTool()
if self.context:
self.afftool = AffordanceTool(context=self.context)
self.afftool.client.affordance.affordance.resetAffordanceConfig() # FIXME: this should be called in afftool constructor
else:
self.afftool = AffordanceTool()
self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
self.afftool.loadObstacleModel("package://"+env_package + "/urdf/" + env_name + ".urdf",
"planning", vf, reduceSizes=reduce_sizes)
......@@ -135,7 +156,7 @@ class AbstractPathPlanner:
else:
self.ps.addPathOptimizer("RandomShortcut")
def solve(self):
def solve(self, display_roadmap = False):
"""
Solve the path planning problem.
q_init and q_goal must have been defined before calling this method
......@@ -147,7 +168,10 @@ class AbstractPathPlanner:
self.ps.setInitialConfig(self.q_init)
self.ps.addGoalConfig(self.q_goal)
self.v(self.q_init)
t = self.ps.solve()
if display_roadmap and self.v.client.gui.getNodeList() is not None:
t = self.v.solveAndDisplay("roadmap", 5, 0.001)
else:
t = self.ps.solve()
print("Guide planning time : ", t)
......
......@@ -23,7 +23,7 @@ class AnymalContactGenerator(AbstractContactGenerator):
self.fullbody.setEndState(self.q_goal, self.end_contacts, normals_end)
def load_fullbody(self):
from hpp.corbaserver.rbprm.anymal import Robot
from anymal_rbprm.anymal import Robot
self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
......
......@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
class AnymalPathPlanner(AbstractPathPlanner):
def __init__(self):
super().__init__()
def __init__(self, context = None):
super().__init__(context)
# set default bounds to a large workspace on x,y with small interval around reference z value
self.root_translation_bounds = [-5., 5., -5., 5., 0.4, 0.5]
# set default used limbs to be both feet
......@@ -19,8 +19,8 @@ class AnymalPathPlanner(AbstractPathPlanner):
self.robot_node_name = "anymal_trunk"
def load_rbprm(self):
from hpp.corbaserver.rbprm.anymal_abstract import Robot
self.rbprmBuilder = Robot()
from anymal_rbprm.anymal_abstract import Robot
self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
def set_joints_bounds(self):
super().set_joints_bounds()
......
......@@ -2,14 +2,28 @@ from hpp.corbaserver.rbprm.scenarios.talos_path_planner import TalosPathPlanner
class PathPlanner(TalosPathPlanner):
def init_problem(self):
self.a_max = 0.1
super().init_problem()
# greatly increase the number of loops of the random shortcut
self.ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 50)
# force the base orientation to follow the direction of motion along the Z axis
self.ps.setParameter("Kinodynamic/forceYawOrientation", True)
def run(self):
self.init_problem()
self.q_init[:2] = [0, 0]
self.q_goal[:2] = [1, 0]
self.q_goal[:2] = [1., 0]
# Constraint the initial orientation when forceYawOrientation = True, expressed as a 3D vector (x,y,z)
#self.q_init[-6:-3] = [0.1, 0, 0]
#self.q_goal[-6:-3] = [0, -0.1, 0]
self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
self.init_planner()
self.init_planner(True, False)
self.solve()
self.display_path()
# self.play_path()
......
......@@ -6,7 +6,7 @@ class PathPlanner(TalosPathPlanner):
from talos_rbprm.talos_abstract import Robot
Robot.urdfName = "talos_trunk_large" # load the model with conservative bounding boxes for trunk
self.robot_node_name = "talos_trunk_large"
self.rbprmBuilder = Robot()
self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
def init_problem(self):
super().init_problem()
......
......@@ -11,7 +11,7 @@ class ContactGenerator(TalosContactGenerator):
# use a model with upscaled collision geometry for the feet
Robot.urdfSuffix += "_safeFeet"
self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof
self.q_ref = self.fullbody.referenceConfig_legsApart[::] + [0] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
def set_joints_bounds(self):
......
......@@ -12,8 +12,8 @@ class PathPlanner(TalosPathPlanner):
self.root_translation_bounds = [-5, 5, -1.5, 1.5, 0.95, 1.3]
self.init_problem()
self.q_init[:3] = [0.16, 0.25, 1.14]
self.q_goal[:3] = [1.09, 0.25, 1.14]
self.q_init[:3] = [0.18, 0.25, 1.14]
self.q_goal[:3] = [1.07, 0.25, 1.14]
self.init_viewer("multicontact/plateforme_surfaces",
reduce_sizes=[0.18, 0, 0],
......
......@@ -9,7 +9,7 @@ class HRP2ContactGenerator(AbstractContactGenerator):
self.robot_node_name = "hrp2_14"
def load_fullbody(self):
from hpp.corbaserver.rbprm.hrp2 import Robot
from hrp2_rbprm.hrp2 import Robot
self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
......
......@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
class HRP2PathPlanner(AbstractPathPlanner):
def __init__(self):
super().__init__()
def __init__(self, context = None):
super().__init__(context)
# set default bounds to a large workspace on x,y with small interval around reference z value
self.root_translation_bounds = [-5., 5., -5., 5., 0.5, 0.8]
# set default used limbs to be both feet
......@@ -19,5 +19,5 @@ class HRP2PathPlanner(AbstractPathPlanner):
self.robot_node_name = "hrp2_trunk_flexible"
def load_rbprm(self):
from hpp.corbaserver.rbprm.hrp2_abstract import Robot
self.rbprmBuilder = Robot()
from hrp2_rbprm.hrp2_abstract import Robot
self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
......@@ -23,7 +23,7 @@ class HyqContactGenerator(AbstractContactGenerator):
self.fullbody.setEndState(self.q_goal, self.end_contacts, normals_end)
def load_fullbody(self):
from hpp.corbaserver.rbprm.hyq import Robot
from hyq_rbprm.hyq import Robot
self.fullbody = Robot()
self.q_ref = self.fullbody.referenceConfig[::] + [0] * self.path_planner.extra_dof
self.weight_postural = self.fullbody.postureWeights[::] + [0] * self.path_planner.extra_dof
......
......@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
class HyqPathPlanner(AbstractPathPlanner):
def __init__(self):
super().__init__()
def __init__(self, context = None):
super().__init__(context)
# set default bounds to a large workspace on x,y with small interval around reference z value
self.root_translation_bounds = [-5., 5., -5., 5., 0.5, 0.75]
# set default used limbs to be both feet
......@@ -19,8 +19,8 @@ class HyqPathPlanner(AbstractPathPlanner):
self.robot_node_name = "hyq_trunk_large"
def load_rbprm(self):
from hpp.corbaserver.rbprm.hyq_abstract import Robot
self.rbprmBuilder = Robot()
from hyq_rbprm.hyq_abstract import Robot
self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
def set_joints_bounds(self):
super().set_joints_bounds()
......
......@@ -2,8 +2,8 @@ from .abstract_path_planner import AbstractPathPlanner
class TalosPathPlanner(AbstractPathPlanner):
def __init__(self):
super().__init__()
def __init__(self, context = None):
super().__init__(context)
# set default bounds to a large workspace on x,y with small interval around reference z value
self.root_translation_bounds = [-5., 5., -5., 5., 0.95, 1.05]
# set default used limbs to be both feet
......@@ -20,7 +20,8 @@ class TalosPathPlanner(AbstractPathPlanner):
def load_rbprm(self):
from talos_rbprm.talos_abstract import Robot
self.rbprmBuilder = Robot()
self.rbprmBuilder = Robot(client=self.hpp_client, clientRbprm=self.rbprm_client)
self.root_translation_bounds[-2:] = [self.rbprmBuilder.ref_height] * 2
def set_joints_bounds(self):
super().set_joints_bounds()
......
import numpy as np
# from hpp.corbaserver.rbprm.hrp2 import Robot as rob
# from hpp.corbaserver.rbprm.tools.obj_to_constraints import load_obj, as_inequalities, rotate_inequalities
# from hpp_centroidal_dynamics import *
# from hpp_spline import *e
from numpy import array, hstack, identity, matrix, ones, vstack, zeros
from scipy.spatial import ConvexHull
# import eigenpy
import cdd
# from hpp_bezier_com_traj import *
# from qp import solve_lp
Id = matrix([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
z = array([0., 0., 1.])
zero3 = zeros(3)
def generators(A, b, Aeq=None, beq=None):
m = np.hstack([b, -A])
matcdd = cdd.Matrix(m)
matcdd.rep_type = cdd.RepType.INEQUALITY
if Aeq is not None:
meq = np.hstack([beq, -Aeq])
matcdd.extend(meq.tolist(), True)
H = cdd.Polyhedron(matcdd)
g = H.get_generators()
return [array(g[el][1:]) for el in range(g.row_size)], H
def filter(pts):
hull = ConvexHull(pts, qhull_options='Q12')
return [pts[i] for i in hull.vertices.tolist()]
def ineq(pts, canonicalize=False):
apts = array(pts)
m = np.hstack([ones((apts.shape[0], 1)), apts])
matcdd = cdd.Matrix(m)
matcdd.rep_type = cdd.RepType.GENERATOR
H = cdd.Polyhedron(matcdd)
bmA = H.get_inequalities()
if canonicalize:
bmA.canonicalize()
Ares = zeros((bmA.row_size, bmA.col_size - 1))
bres = zeros(bmA.row_size)
for i in range(bmA.row_size):
bmAl = array(bmA[i])
Ares[i, :] = -bmAl[1:]
bres[i] = bmAl[0]
return Ares, bres
def ineqQHull(hull):
A = hull.equations[:, :-1]
b = -hull.equations[:, -1]
return A, b
def canon(A, b):
m = np.hstack([b, -A])
matcdd = cdd.Matrix(m)
matcdd.rep_type = 1
H = cdd.Polyhedron(matcdd)
bmA = H.get_inequalities()
# bmA.canonicalize()
Ares = zeros((bmA.row_size, bmA.col_size - 1))
bres = zeros((bmA.row_size, 1))
for i in range(bmA.row_size):
# print("line ", array(bmA[i]))
# print("A ", A[i][:])
# print("b ", b[i])
bmAl = array(bmA[i])
Ares[i, :] = -bmAl[1:]
bres[i] = bmAl[0]
# print("Ares ",Ares[i,:])
# print("bres ",bres[i])
return Ares, bres