def homing(q=None, pose=None):
pose = pose or "home"
rospy.init_node('posePublisher', anonymous=True)
pub = rospy.Publisher('pose_state', JointState, queue_size=10)
rate = rospy.Rate(1) # default is 10 Hz
state_str = JointState()
state_str.header = Header()
now = rospy.get_rostime()
state_str.header.stamp.secs = now.secs
state_str.header.stamp.nsecs = now.nsecs
centauro = config.HomePose(pose=pose)
state_str.name = centauro.getName()
if not q:
state_str.position = centauro.getValue()
else:
state_str.position = q
# VARIABLE FOR TERMINATING
ter_publish = False
while not ter_publish:
connections = pub.get_num_connections()
# print "number of connections to this publisher: %s" %connections
if connections > 0:
# rospy.loginfo(state_str) # use for debugging
pub.publish(state_str)
ter_publish = True
# print "Message Published. Closing Program..."
else:
rate.sleep()
def posePublisher(pose,pubrate=120,init_pose=None,full_model=False,rostopic='pose_state'):
####################################################
# This part is to add extra values to the print statements
if full_model:
if init_pose is None:
init_pose = 'home'
centauro = config.HomePose(pose=init_pose)
full_name = centauro.getName()
# print len(full_name)
nj_extra = len(full_name) - len(pose.name)
index = [i for i, x in enumerate(full_name) if x in pose.name]
name_extra = [i for j, i in enumerate(full_name) if j not in index]
q_extra = np.delete(centauro.getValue(),index)
else:
nj_extra = 0
#####################################################
pub = rospy.Publisher(rostopic, JointState, queue_size=10)
rospy.init_node('posePublisher', anonymous=True)
rate = rospy.Rate(pubrate) # default is 10 Hz
# print pose.rate
state_str = JointState()
state_str.header = Header()
iteration = 0
# print np.shape(pose.q)
send_velocity = False
while not rospy.is_shutdown() and iteration < pose.q.shape[0]:
now = rospy.get_rostime()
state_str.header.seq = iteration
state_str.header.stamp.secs = now.secs
state_str.header.stamp.nsecs = now.nsecs
state_str.name = pose.name
state_str.position = pose.q[iteration,:]
if nj_extra > 0:
state_str.name = pose.name + name_extra
state_str.position = np.concatenate((state_str.position,q_extra))
zerovec = np.zeros(nj_extra)
if send_velocity:
if np.shape(pose.qdot)[0] > 1:
state_str.velocity = pose.qdot[iteration,:]
if nj_extra > 0:
state_str.velocity = np.concatenate((state_str.velocity,zerovec))
if np.shape(pose.tau)[0] > 1:
state_str.effort = pose.tau[iteration,:]
if nj_extra > 0:
state_str.effort = np.concatenate((state_str.effort,zerovec))
else:
pass
# rospy.loginfo(state_str) # use for debugging
pub.publish(state_str)
iteration += 1
rate.sleep()
# print ubh # DEFINE CASADI FUNCTION & END-EFFECTOR end_effector = 'arm1_8' invDyn = Function.deserialize(kindyn.rnea()) forKin = Function.deserialize(kindyn.fk(end_effector)) jacEE = Function.deserialize(kindyn.jacobian(end_effector)) inertiaJS = Function.deserialize(kindyn.crba()) # print forKin # print inertiaJS # INITIAL JOINT STATE # p_start = [0.7, 0.16, 1.26] q_0 = config.HomePose(pose=init_pose, name=joint_str).getValue() # cont, q_0 = invKyn.invKin(p_des=p_start,fk=forKin,frame=end_effector,j_str=joint_str,q_init=q_0,animate=False,T=2) # q_0 = q_0.tolist() # q_0 = [0.0,0.0,0.0] qdot_0 = np.zeros(nj).tolist() qddot_0 = np.zeros(nj).tolist() limit_workspace = True p_0 = forKin(q=q_0)['ee_pos'].full().flatten() R_0 = forKin(q=q_0)['ee_rot'].full() p_torso = [0.2, 0.0, 1.19045] ball_radius = 0.75 / 2 z_adjust = -0.113 # [m] theta_lb = math.pi / 2 theta_ub = math.pi
dt_ub = (Tf_ub / N_stage).flatten().tolist()
# print ubh
# DEFINE CASADI FUNCTION & END-EFFECTOR
end_effector = 'arm1_8'
invDyn = Function.deserialize(kindyn.rnea())
forKin = Function.deserialize(kindyn.fk(end_effector))
jacEE = Function.deserialize(kindyn.jacobian(end_effector))
inertiaJS = Function.deserialize(kindyn.crba())
# print forKin
# print inertiaJS
# INITIAL JOINT STATE
q_0 = config.HomePose(pose="home", name=joint_str).getValue()
# q_0 = [0.0,0.0,0.0]
qdot_0 = np.zeros(nj).tolist()
qddot_0 = np.zeros(nj).tolist()
# p_end = [1.0, 0.0, 1.35]
# p_end = [1.00696, 0.0, 1.23751]
# p_end = [1.00696, 0.0724726, 1.23751]
p_end = [0.9, 0.0, 1.24]
# p_end = [0.95, 0.0, 1.24]
cont, q_end = invKyn.invKin(p_des=p_end,
fk=forKin,
frame=end_effector,
j_str=joint_str,
q_init=q_0,
animate=False,
