"""universal_robot_ros controller."""
import rospy
from controller import Robot
from joint_state_publisher import JointStatePublisher
from trajectory_follower import TrajectoryFollower
from rosgraph_msgs.msg import Clock
rospy.init_node('ur_driver', disable_signals=True)
jointPrefix = rospy.get_param('prefix', '')
if jointPrefix:
print('Setting prefix to %s' % jointPrefix)
robot = Robot()
jointStatePublisher = JointStatePublisher(robot, jointPrefix)
trajectoryFollower = TrajectoryFollower(robot, jointStatePublisher, jointPrefix)
trajectoryFollower.start()
# we want to use simulation time for ROS
clockPublisher = rospy.Publisher('clock', Clock, queue_size=1)
if not rospy.get_param('use_sim_time', False):
rospy.logwarn('use_sim_time is not set!')
timestep = int(robot.getBasicTimeStep())
while robot.step(timestep) != -1 and not rospy.is_shutdown():
jointStatePublisher.publish()
trajectoryFollower.update()
# pulish simulation clock
msg = Clock()
parser = argparse.ArgumentParser()
parser.add_argument('--node-name',
dest='nodeName',
default='ur_driver',
help='Specifies the name of the node.')
arguments, unknown = parser.parse_known_args()
rospy.init_node(arguments.nodeName, disable_signals=True)
jointPrefix = rospy.get_param('prefix', '')
if jointPrefix:
print('Setting prefix to %s' % jointPrefix)
robot = Robot()
nodeName = arguments.nodeName + '/' if arguments.nodeName != 'ur_driver' else ''
jointStatePublisher = JointStatePublisher(robot, jointPrefix, nodeName)
trajectoryFollower = TrajectoryFollower(robot, jointStatePublisher,
jointPrefix, nodeName)
trajectoryFollower.start()
# we want to use simulation time for ROS
clockPublisher = rospy.Publisher('clock', Clock, queue_size=1)
if not rospy.get_param('use_sim_time', False):
rospy.logwarn('use_sim_time is not set!')
timestep = int(robot.getBasicTimeStep())
while robot.step(timestep) != -1 and not rospy.is_shutdown():
jointStatePublisher.publish()
trajectoryFollower.update()
# pulish simulation clock
# compute foot position to be in contact with the ground
rbd.forwardKinematics(hrp4.mb, hrp4.mbc)
tz = -hrp4.surfaces['LeftFoot'].X_0_s(hrp4).translation().z()
tx = -hrp4.surfaces['LeftFoot'].X_0_s(
hrp4).translation().x() #zero the feet surface for the wPG
hrp4_q = rbdList(hrp4.mbc.q)
hrp4_q[0] = [1., 0., 0., 0., tx, 0., tz]
hrp4.mbc.q = hrp4_q
# print len(rbdList(hrp4.mbc.q))
# compute init fk and fv
for r in robots.robots:
rbd.forwardKinematics(r.mb, r.mbc)
rbd.forwardVelocity(r.mb, r.mbc)
hrp4Jsp = JointStatePublisher(hrp4)
# create stabilizer helper
hrp4Stab = stabilizerMsg(hrp4)
# create solver
qpsolver = MRQPSolver(robots, timeStep)
# add dynamics constraint to QPSolver
# Use 50% of the velocity limits cf Sebastien Langagne.
contactConstraint = ContactConstraint(timeStep, ContactConstraint.Position)
# contactConstraint = ContactConstraint(timeStep, ContactConstraint.Velocity)
dynamicsConstraint1 = DynamicsConstraint(robots,
hrp4_index,
timeStep,
damper=(0.1, 0.01, 0.5),