# Vxc2, Vyc2&Vxc3, Vyc3 calculation problem

Hi, I’m a bit confused about the given solution about the velocity of two link’s mass center
The solution is Vxc2 = vx2 + 0.5 * math.sin(th2)

I get the Vxc2 from xc2 = x2 + 0.5 * math.sin(th2) by derivating it, and my answer is:
vxc2 = vx2 + w20.5math.cos(th2)
vyc2 = vy2 - w20.5math.sin(th2)
and same for link3 mass center.

But the plot for velocity and kinetic energy looks different to the solution. Could someone help me?

Hello @youngyangcs,

Can you please specify which exercise is this?

Best,

That is Exercise3.1

Cheers

Hello @youngyangcs,

I think you are totally right and that might be a typo in the notebooks. I’ve tested with the corrected expressions and I’m actually getting the expected plots. I’m using the following code:

``````#!/usr/bin/env python

import math, rospy, numpy as np

from std_msgs.msg import Float32MultiArray

def pose2d(pose):
x, y = pose.position.x, pose.position.z
theta = 2 * math.atan2(pose.orientation.y, pose.orientation.w)
return (x, y, theta)

def twist2d(twist):
return (twist.linear.x, twist.linear.z, twist.angular.y)

def callback(msg_in):
global pub
x2, y2, th2 = pose2d(msg_in.pose)
vx2, vy2, w2 = twist2d(msg_in.twist)
x3, y3, th3 = pose2d(msg_in.pose)
vx3, vy3, w3 = twist2d(msg_in.twist)
xc2 = x2 + 0.5 * math.sin(th2)
yc2 = y2 + 0.5 * math.cos(th2)
xc3 = x3 + 0.5 * math.sin(th3)
yc3 = y3 + 0.5 * math.cos(th3)
#vxc2 = vx2 - w2*(yc2-y2)
#vyc2 = vy2 - w2*(xc2-x2)
#vxc3 = vx3 - w3*(yc3-y3)
#vyc3 = vy3 - w3*(xc3-x3)
vxc2 = vx2 + w2*(0.5*math.cos(th2))
vyc2 = vy2 - w2*(0.5*math.sin(th2))
vxc3 = vx3 + w3*(0.5*math.cos(th3))
vyc3 = vy3 - w3*(0.5*math.sin(th3))
m = 1.0
g = 9.81
I = 0.084
pe = m*g*(yc2+yc3)
ke = m*(vxc2*vxc2+vyc2*vyc2) / 2 + m*(vxc3*vxc3+vyc3*vyc3) / 2 + I*np.array(w2)*np.array(w2) / 2 + I*np.array(w3)*np.array(w3) / 2

data = [xc2,yc2,th2,vxc2,vyc2,w2,xc3,yc3,th3,vxc3,vyc3,w3,pe,ke]
msg_out = Float32MultiArray(data=data)
pub.publish(msg_out)

if __name__ == '__main__':
rospy.init_node('dynamics_monitor', anonymous=True)