def publishTransform(publisher, transform):
t = TransformROS()
t.rotation.x = transform.quaternion.array[0]
t.rotation.y = transform.quaternion.array[1]
t.rotation.z = transform.quaternion.array[2]
t.rotation.w = transform.quaternion.array[3]
t.translation.x = transform.translation[0]
t.translation.y = transform.translation[1]
t.translation.z = transform.translation[2]
publisher.publish(t)
client = CorbaClient()
bag = rosbag.Bag("/usr/localDev/rosbag-calib/pyrene-calib.bag")
Robot.packageName = "agimus_demos"
Robot.urdfName = "talos"
Robot.urdfSuffix = "_calibration_camera"
Robot.srdfSuffix = ""
robot = Robot("talos", "talos", rootJointType="freeflyer", client=client)
q = robot.getCurrentConfig()
pub_cMo = rospy.Publisher("/camera_object", TransformROS, queue_size=100)
pub_fMe = rospy.Publisher("/world_effector", TransformROS, queue_size=100)
rospy.init_node("pose_sender", anonymous=True)
i = 0
from hpp import Transform
from hpp.corbaserver import loadServerPlugin
from hpp.corbaserver.manipulation import Constraints, ConstraintGraph, \
ProblemSolver, Rule
from hpp.corbaserver.manipulation.robot import CorbaClient, HumanoidRobot
from hpp.gepetto.manipulation import ViewerFactory
from hpp.corbaserver.manipulation.constraint_graph_factory import \
ConstraintGraphFactory
from common_hpp import createGripperLockedJoints, createLeftArmLockedJoints, \
createRightArmLockedJoints, createQuasiStaticEquilibriumConstraint, \
createWaistYawConstraint, defaultContext, shrinkJointRange
loadServerPlugin(defaultContext, "manipulation-corba.so")
client = CorbaClient(context=defaultContext)
client.manipulation.problem.resetProblem()
# parse arguments
p = argparse.ArgumentParser(
description='Procuce motion to calibrate arms and camera')
p.add_argument('--arm',
type=str,
metavar='arm',
default=None,
help="which arm: 'right' or 'left'")
p.add_argument('--N',
type=int,
metavar='N',
default=0,
help="number of configurations generated")
'Initialize estimation for the demo of Pyrene manipulating a box')
p.add_argument('--ros-param',
type=str,
metavar='ros_param',
help="The name of the ROS param containing the URDF.")
args = p.parse_args()
loadServerPlugin("estimation", "manipulation-corba.so")
footPlacement = True
projectRobotOnFloor = True
comConstraint = False
constantWaistYaw = True
fixedArmWhenGrasping = True
clients = CorbaClient(context="estimation")
if not clients.manipulation.problem.selectProblem("estimation"):
clients.manipulation.problem.resetProblem()
robot, ps, vf, table, objects = makeRobotProblemAndViewerFactory(
clients, rolling_table=True, rosParam=args.ros_param)
q_neutral = robot.getCurrentConfig()
ps.addPartialCom("talos", ["talos/root_joint"])
ps.addPartialCom("talos_box", ["talos/root_joint", "box/root_joint"])
# Static stability constraint
robot.createStaticStabilityConstraint("balance/", "talos", robot.leftAnkle,
robot.rightAnkle, init_conf)
foot_placement = ["balance/pose-left-foot"]
help="The name of the ROS param containing the URDF.")
args = p.parse_args ()
if args.context != defaultContext:
createContext (args.context)
print("context=" + args.context)
loadServerPlugin (args.context, "manipulation-corba.so")
isSimulation = args.context == "simulation"
footPlacement = not isSimulation
comConstraint = not isSimulation
constantWaistYaw = not isSimulation
fixedArmWhenGrasping = not isSimulation
client = CorbaClient(context=args.context)
newProblem(client=client.manipulation, name=args.context)
robot, ps, vf, table, objects = makeRobotProblemAndViewerFactory(client, rolling_table=True,
rosParam=args.ros_param)
if isSimulation:
ps.setMaxIterProjection (1)
q_neutral = robot.getCurrentConfig()
# Set robot to neutral configuration before building constraint graph
robot.setCurrentConfig(q_neutral)
ps.setDefaultLineSearchType ("ErrorNormBased")
ps.setMaxIterPathPlanning (40)
# /usr/bin/env python
import numpy as np
from hpp import Transform
from hpp.corbaserver.manipulation import ProblemSolver
from hpp.corbaserver.manipulation.robot import CorbaClient
from common_hpp import *
clients = CorbaClient(postContextId="_estimation")
clients.manipulation.problem.resetProblem()
robot, ps, vf = makeRobotProblemAndViewerFactory(clients)
ps.setErrorThreshold(1e-2)
q_init = robot.getCurrentConfig()
ps.addPartialCom("talos", ["talos/root_joint"])
ps.addPartialCom("talos_box", ["talos/root_joint", "box/root_joint"])
# ps.createStaticStabilityConstraints ("balance", half_sitting, "talos", ProblemSolver.FIXED_ON_THE_GROUND)
# foot_placement = [ "balance/pose-left-foot", "balance/pose-right-foot" ]
# foot_placement_complement = [ ]
ps.createStaticStabilityConstraints("balance", half_sitting, "talos",
ProblemSolver.SLIDING)
foot_placement = [
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot",
]
foot_placement_complement = [
import numpy as np
from hpp import Quaternion, Transform
from hpp.corbaserver.manipulation import Constraints, ProblemSolver
from hpp.corbaserver.manipulation.robot import CorbaClient
from hpp.corbaserver import loadServerPlugin
from common_hpp import *
loadServerPlugin("corbaserver", "manipulation-corba.so")
footPlacement = True
comConstraint = True
constantWaistYaw = True
fixedArmWhenGrasping = True
client = CorbaClient(context="corbaserver")
client.manipulation.problem.resetProblem()
robot, ps, vf, table, objects = makeRobotProblemAndViewerFactory(client)
q_neutral = robot.getCurrentConfig()
ps.addPartialCom("talos", ["talos/root_joint"])
ps.addPartialCom("talos_box", ["talos/root_joint", "box/root_joint"])
# Static stability constraint
robot.createStaticStabilityConstraint("balance/", "talos", robot.leftAnkle,
robot.rightAnkle, init_conf)
foot_placement = ["balance/pose-left-foot", "balance/pose-right-foot"]
foot_placement_complement = []
# /usr/bin/env python
import numpy as np
from hpp import Transform
from hpp.corbaserver.manipulation import ConstraintGraph, ProblemSolver, Rule
from hpp.corbaserver.manipulation.robot import CorbaClient, Robot
from hpp.gepetto.manipulation import ViewerFactory
import CORBA
clients = CorbaClient()
clients.manipulation.problem.resetProblem()
Robot.packageName = "talos_data"
Robot.urdfName = "talos"
Robot.urdfSuffix = "_full_v2"
Robot.srdfSuffix = ""
class Box(object):
rootJointType = "freeflyer"
packageName = "hpp_tutorial"
meshPackageName = "hpp_tutorial"
urdfName = "cup"
urdfSuffix = ""
srdfSuffix = ""
handles = ["box/top", "box/bottom"]
contacts = ["box/box_surface"]
class Brick(object):
rootJointType = "freeflyer"
# /usr/bin/env python
import numpy as np
from hpp import Transform
from hpp.corbaserver.manipulation import ProblemSolver
from hpp.corbaserver.manipulation.robot import CorbaClient
from common_hpp import *
clients = CorbaClient(postContextId="")
clients.manipulation.problem.resetProblem()
robot, ps, vf = makeRobotProblemAndViewerFactory(clients)
q_init = robot.getCurrentConfig()
ps.addPartialCom("talos", ["talos/root_joint"])
ps.addPartialCom("talos_box", ["talos/root_joint", "box/root_joint"])
# ps.createStaticStabilityConstraints ("balance", half_sitting, "talos", ProblemSolver.FIXED_ON_THE_GROUND)
# foot_placement = [ "balance/pose-left-foot", "balance/pose-right-foot" ]
# foot_placement_complement = [ ]
ps.createStaticStabilityConstraints("balance", half_sitting, "talos",
ProblemSolver.SLIDING)
foot_placement = [
"balance/relative-orientation",
"balance/relative-position",
"balance/orientation-left-foot",
"balance/position-left-foot",
]
foot_placement_complement = [
"balance/orientation-left-foot-complement",